Merge ../linux-2.6

593 files changed, 43342 insertions, 7659 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index 808e0ae66aa..54ec5e718c0 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -5,7 +5,7 @@
 # Rewritten to use lists instead of if-statements.
 #
 
-obj-$(CONFIG_HAVE_GPIO_LIB)	+= gpio/
+obj-y				+= gpio/
 obj-$(CONFIG_PCI)		+= pci/
 obj-$(CONFIG_PARISC)		+= parisc/
 obj-$(CONFIG_RAPIDIO)		+= rapidio/
diff --git a/drivers/acpi/namespace/nsnames.c b/drivers/acpi/namespace/nsnames.c
index cffef1bcbdb..549db42f16c 100644
--- a/drivers/acpi/namespace/nsnames.c
+++ b/drivers/acpi/namespace/nsnames.c
@@ -137,6 +137,10 @@ char *acpi_ns_get_external_pathname(struct acpi_namespace_node *node)
 	/* Calculate required buffer size based on depth below root */
 
 	size = acpi_ns_get_pathname_length(node);
+	if (!size) {
+		ACPI_ERROR((AE_INFO, "Invalid node failure"));
+		return_PTR(NULL);
+	}
 
 	/* Allocate a buffer to be returned to caller */
 
@@ -229,6 +233,10 @@ acpi_ns_handle_to_pathname(acpi_handle target_handle,
 	/* Determine size required for the caller buffer */
 
 	required_size = acpi_ns_get_pathname_length(node);
+	if (!required_size) {
+		ACPI_ERROR((AE_INFO, "Invalid node failure"));
+		return_ACPI_STATUS(AE_ERROR);
+	}
 
 	/* Validate/Allocate/Clear caller buffer */
 
diff --git a/drivers/acpi/pci_link.c b/drivers/acpi/pci_link.c
index 233c40c5168..89f3b2abfdc 100644
--- a/drivers/acpi/pci_link.c
+++ b/drivers/acpi/pci_link.c
@@ -113,20 +113,23 @@ acpi_pci_link_check_possible(struct acpi_resource *resource, void *context)
 
 	switch (resource->type) {
 	case ACPI_RESOURCE_TYPE_START_DEPENDENT:
+	case ACPI_RESOURCE_TYPE_END_TAG:
 		return AE_OK;
 	case ACPI_RESOURCE_TYPE_IRQ:
 		{
 			struct acpi_resource_irq *p = &resource->data.irq;
 			if (!p || !p->interrupt_count) {
-				printk(KERN_WARNING PREFIX "Blank IRQ resource\n");
+				ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+						  "Blank _PRS IRQ resource\n"));
 				return AE_OK;
 			}
 			for (i = 0;
 			     (i < p->interrupt_count
 			      && i < ACPI_PCI_LINK_MAX_POSSIBLE); i++) {
 				if (!p->interrupts[i]) {
-					printk(KERN_WARNING PREFIX "Invalid IRQ %d\n",
-						      p->interrupts[i]);
+					printk(KERN_WARNING PREFIX
+					       "Invalid _PRS IRQ %d\n",
+					       p->interrupts[i]);
 					continue;
 				}
 				link->irq.possible[i] = p->interrupts[i];
@@ -143,15 +146,16 @@ acpi_pci_link_check_possible(struct acpi_resource *resource, void *context)
 			    &resource->data.extended_irq;
 			if (!p || !p->interrupt_count) {
 				printk(KERN_WARNING PREFIX
-					      "Blank EXT IRQ resource\n");
+					      "Blank _PRS EXT IRQ resource\n");
 				return AE_OK;
 			}
 			for (i = 0;
 			     (i < p->interrupt_count
 			      && i < ACPI_PCI_LINK_MAX_POSSIBLE); i++) {
 				if (!p->interrupts[i]) {
-					printk(KERN_WARNING PREFIX "Invalid IRQ %d\n",
-						      p->interrupts[i]);
+					printk(KERN_WARNING PREFIX
+					       "Invalid _PRS IRQ %d\n",
+					       p->interrupts[i]);
 					continue;
 				}
 				link->irq.possible[i] = p->interrupts[i];
@@ -163,7 +167,8 @@ acpi_pci_link_check_possible(struct acpi_resource *resource, void *context)
 			break;
 		}
 	default:
-		printk(KERN_ERR PREFIX "Resource is not an IRQ entry\n");
+		printk(KERN_ERR PREFIX "_PRS resource type 0x%x isn't an IRQ\n",
+		       resource->type);
 		return AE_OK;
 	}
 
@@ -199,6 +204,9 @@ acpi_pci_link_check_current(struct acpi_resource *resource, void *context)
 
 
 	switch (resource->type) {
+	case ACPI_RESOURCE_TYPE_START_DEPENDENT:
+	case ACPI_RESOURCE_TYPE_END_TAG:
+		return AE_OK;
 	case ACPI_RESOURCE_TYPE_IRQ:
 		{
 			struct acpi_resource_irq *p = &resource->data.irq;
@@ -208,7 +216,7 @@ acpi_pci_link_check_current(struct acpi_resource *resource, void *context)
 				 * particularly those those w/ _STA disabled
 				 */
 				ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-						  "Blank IRQ resource\n"));
+						  "Blank _CRS IRQ resource\n"));
 				return AE_OK;
 			}
 			*irq = p->interrupts[0];
@@ -224,7 +232,7 @@ acpi_pci_link_check_current(struct acpi_resource *resource, void *context)
 				 * return at least 1 IRQ
 				 */
 				printk(KERN_WARNING PREFIX
-					      "Blank EXT IRQ resource\n");
+					      "Blank _CRS EXT IRQ resource\n");
 				return AE_OK;
 			}
 			*irq = p->interrupts[0];
@@ -232,10 +240,11 @@ acpi_pci_link_check_current(struct acpi_resource *resource, void *context)
 		}
 		break;
 	default:
-		printk(KERN_ERR PREFIX "Resource %d isn't an IRQ\n", resource->type);
-	case ACPI_RESOURCE_TYPE_END_TAG:
+		printk(KERN_ERR PREFIX "_CRS resource type 0x%x isn't an IRQ\n",
+		       resource->type);
 		return AE_OK;
 	}
+
 	return AE_CTRL_TERMINATE;
 }
 
diff --git a/drivers/acpi/pci_slot.c b/drivers/acpi/pci_slot.c
index b9ab030a52d..dd376f7ad09 100644
--- a/drivers/acpi/pci_slot.c
+++ b/drivers/acpi/pci_slot.c
@@ -6,8 +6,8 @@
  *  Thanks to Kenji Kaneshige <kaneshige.kenji@jp.fujitsu.com> for code
  *  review and fixes.
  *
- *  Copyright (C) 2007 Alex Chiang <achiang@hp.com>
- *  Copyright (C) 2007 Hewlett-Packard Development Company, L.P.
+ *  Copyright (C) 2007-2008 Hewlett-Packard Development Company, L.P.
+ *  	Alex Chiang <achiang@hp.com>
  *
  *  This program is free software; you can redistribute it and/or modify it
  *  under the terms and conditions of the GNU General Public License,
@@ -158,6 +158,7 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	if (IS_ERR(pci_slot)) {
 		err("pci_create_slot returned %ld\n", PTR_ERR(pci_slot));
 		kfree(slot);
+		return AE_OK;
 	}
 
 	slot->root_handle = parent_context->root_handle;
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index d592dbb1d12..b7f2963693a 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -272,6 +272,8 @@ static atomic_t c3_cpu_count;
 /* Common C-state entry for C2, C3, .. */
 static void acpi_cstate_enter(struct acpi_processor_cx *cstate)
 {
+	/* Don't trace irqs off for idle */
+	stop_critical_timings();
 	if (cstate->entry_method == ACPI_CSTATE_FFH) {
 		/* Call into architectural FFH based C-state */
 		acpi_processor_ffh_cstate_enter(cstate);
@@ -284,6 +286,7 @@ static void acpi_cstate_enter(struct acpi_processor_cx *cstate)
 		   gets asserted in time to freeze execution properly. */
 		unused = inl(acpi_gbl_FADT.xpm_timer_block.address);
 	}
+	start_critical_timings();
 }
 #endif /* !CONFIG_CPU_IDLE */
 
@@ -1418,6 +1421,8 @@ static inline void acpi_idle_update_bm_rld(struct acpi_processor *pr,
  */
 static inline void acpi_idle_do_entry(struct acpi_processor_cx *cx)
 {
+	/* Don't trace irqs off for idle */
+	stop_critical_timings();
 	if (cx->entry_method == ACPI_CSTATE_FFH) {
 		/* Call into architectural FFH based C-state */
 		acpi_processor_ffh_cstate_enter(cx);
@@ -1432,6 +1437,7 @@ static inline void acpi_idle_do_entry(struct acpi_processor_cx *cx)
 		   gets asserted in time to freeze execution properly. */
 		unused = inl(acpi_gbl_FADT.xpm_timer_block.address);
 	}
+	start_critical_timings();
 }
 
 /**
diff --git a/drivers/acpi/sleep/main.c b/drivers/acpi/sleep/main.c
index 0489a7d1d42..d13194a031b 100644
--- a/drivers/acpi/sleep/main.c
+++ b/drivers/acpi/sleep/main.c
@@ -280,9 +280,36 @@ static struct platform_suspend_ops acpi_suspend_ops_old = {
 	.end = acpi_pm_end,
 	.recover = acpi_pm_finish,
 };
+
+static int __init init_old_suspend_ordering(const struct dmi_system_id *d)
+{
+	old_suspend_ordering = true;
+	return 0;
+}
+
+static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
+	{
+	.callback = init_old_suspend_ordering,
+	.ident = "Abit KN9 (nForce4 variant)",
+	.matches = {
+		DMI_MATCH(DMI_BOARD_VENDOR, "http://www.abit.com.tw/"),
+		DMI_MATCH(DMI_BOARD_NAME, "KN9 Series(NF-CK804)"),
+		},
+	},
+	{},
+};
 #endif /* CONFIG_SUSPEND */
 
 #ifdef CONFIG_HIBERNATION
+static unsigned long s4_hardware_signature;
+static struct acpi_table_facs *facs;
+static bool nosigcheck;
+
+void __init acpi_no_s4_hw_signature(void)
+{
+	nosigcheck = true;
+}
+
 static int acpi_hibernation_begin(void)
 {
 	acpi_target_sleep_state = ACPI_STATE_S4;
@@ -316,6 +343,12 @@ static void acpi_hibernation_leave(void)
 	acpi_enable();
 	/* Reprogram control registers and execute _BFS */
 	acpi_leave_sleep_state_prep(ACPI_STATE_S4);
+	/* Check the hardware signature */
+	if (facs && s4_hardware_signature != facs->hardware_signature) {
+		printk(KERN_EMERG "ACPI: Hardware changed while hibernated, "
+			"cannot resume!\n");
+		panic("ACPI S4 hardware signature mismatch");
+	}
 }
 
 static void acpi_pm_enable_gpes(void)
@@ -516,6 +549,8 @@ int __init acpi_sleep_init(void)
 	u8 type_a, type_b;
 #ifdef CONFIG_SUSPEND
 	int i = 0;
+
+	dmi_check_system(acpisleep_dmi_table);
 #endif
 
 	if (acpi_disabled)
@@ -544,6 +579,13 @@ int __init acpi_sleep_init(void)
 			&acpi_hibernation_ops_old : &acpi_hibernation_ops);
 		sleep_states[ACPI_STATE_S4] = 1;
 		printk(" S4");
+		if (!nosigcheck) {
+			acpi_get_table_by_index(ACPI_TABLE_INDEX_FACS,
+				(struct acpi_table_header **)&facs);
+			if (facs)
+				s4_hardware_signature =
+					facs->hardware_signature;
+		}
 	}
 #endif
 	status = acpi_get_sleep_type_data(ACPI_STATE_S5, &type_a, &type_b);
diff --git a/drivers/acpi/system.c b/drivers/acpi/system.c
index d8e3f153b29..91dec448b3e 100644
--- a/drivers/acpi/system.c
+++ b/drivers/acpi/system.c
@@ -26,6 +26,7 @@
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
 #include <linux/init.h>
+#include <linux/string.h>
 #include <asm/uaccess.h>
 
 #include <acpi/acpi_drivers.h>
diff --git a/drivers/acpi/tables/tbfadt.c b/drivers/acpi/tables/tbfadt.c
index ccb5b64bbef..a4a41ba2484 100644
--- a/drivers/acpi/tables/tbfadt.c
+++ b/drivers/acpi/tables/tbfadt.c
@@ -124,7 +124,7 @@ static struct acpi_fadt_info fadt_info_table[] = {
 
 static void inline
 acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
-			     u8 byte_width, u64 address)
+			     u8 bit_width, u64 address)
 {
 
 	/*
@@ -136,7 +136,7 @@ acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
 	/* All other fields are byte-wide */
 
 	generic_address->space_id = ACPI_ADR_SPACE_SYSTEM_IO;
-	generic_address->bit_width = byte_width << 3;
+	generic_address->bit_width = bit_width;
 	generic_address->bit_offset = 0;
 	generic_address->access_width = 0;
 }
@@ -343,11 +343,9 @@ static void acpi_tb_convert_fadt(void)
 	 *
 	 * The PM event blocks are split into two register blocks, first is the
 	 * PM Status Register block, followed immediately by the PM Enable Register
-	 * block. Each is of length (xpm1x_event_block.bit_width/2)
+	 * block. Each is of length (pm1_event_length/2)
 	 */
-	WARN_ON(ACPI_MOD_16(acpi_gbl_FADT.xpm1a_event_block.bit_width));
-	pm1_register_length = (u8) ACPI_DIV_16(acpi_gbl_FADT
-					       .xpm1a_event_block.bit_width);
+	pm1_register_length = (u8) ACPI_DIV_2(acpi_gbl_FADT.pm1_event_length);
 
 	/* The PM1A register block is required */
 
@@ -362,17 +360,14 @@ static void acpi_tb_convert_fadt(void)
 	/* The PM1B register block is optional, ignore if not present */
 
 	if (acpi_gbl_FADT.xpm1b_event_block.address) {
-		WARN_ON(ACPI_MOD_16(acpi_gbl_FADT.xpm1b_event_block.bit_width));
-		pm1_register_length = (u8) ACPI_DIV_16(acpi_gbl_FADT
-						       .xpm1b_event_block
-						       .bit_width);
 		acpi_tb_init_generic_address(&acpi_gbl_xpm1b_enable,
 					     pm1_register_length,
 					     (acpi_gbl_FADT.xpm1b_event_block.
 					      address + pm1_register_length));
 		/* Don't forget to copy space_id of the GAS */
 		acpi_gbl_xpm1b_enable.space_id =
-		    acpi_gbl_FADT.xpm1b_event_block.space_id;
+		    acpi_gbl_FADT.xpm1a_event_block.space_id;
+
 	}
 }
 
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index 30a34133793..912703691d3 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -769,6 +769,47 @@ static void acpi_thermal_run(unsigned long data)
 		acpi_os_execute(OSL_GPE_HANDLER, acpi_thermal_check, (void *)data);
 }
 
+static void acpi_thermal_active_off(void *data)
+{
+	int result = 0;
+	struct acpi_thermal *tz = data;
+	int i = 0;
+	int j = 0;
+	struct acpi_thermal_active *active = NULL;
+
+	if (!tz) {
+		printk(KERN_ERR PREFIX "Invalid (NULL) context\n");
+		return;
+	}
+
+	result = acpi_thermal_get_temperature(tz);
+	if (result)
+		return;
+
+	for (i = 0; i < ACPI_THERMAL_MAX_ACTIVE; i++) {
+		active = &(tz->trips.active[i]);
+		if (!active || !active->flags.valid)
+			break;
+		if (tz->temperature >= active->temperature) {
+			/*
+			 * If the thermal temperature is greater than the
+			 * active threshod, unnecessary to turn off the
+			 * the active cooling device.
+			 */
+			continue;
+		}
+		/*
+		 * Below Threshold?
+		 * ----------------
+		 * Turn OFF all cooling devices associated with this
+		 * threshold.
+		 */
+		for (j = 0; j < active->devices.count; j++)
+			result = acpi_bus_set_power(active->devices.handles[j],
+						    ACPI_STATE_D3);
+	}
+}
+
 static void acpi_thermal_check(void *data)
 {
 	int result = 0;
@@ -1624,6 +1665,8 @@ static int acpi_thermal_add(struct acpi_device *device)
 
 	init_timer(&tz->timer);
 
+	acpi_thermal_active_off(tz);
+
 	acpi_thermal_check(tz);
 
 	status = acpi_install_notify_handler(device->handle,
diff --git a/drivers/acpi/utilities/utalloc.c b/drivers/acpi/utilities/utalloc.c
index 3dfb8a442b2..e7bf34a7b1d 100644
--- a/drivers/acpi/utilities/utalloc.c
+++ b/drivers/acpi/utilities/utalloc.c
@@ -242,6 +242,10 @@ acpi_ut_initialize_buffer(struct acpi_buffer * buffer,
 {
 	acpi_status status = AE_OK;
 
+	if (!required_length) {
+		WARN_ON(1);
+		return AE_ERROR;
+	}
 	switch (buffer->length) {
 	case ACPI_NO_BUFFER:
 
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 37b9e16710d..e8a51a1700f 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -741,7 +741,7 @@ static void acpi_video_device_find_cap(struct acpi_video_device *device)
 
 	max_level = acpi_video_init_brightness(device);
 
-	if (device->cap._BCL && device->cap._BCM && device->cap._BQC && max_level > 0){
+	if (device->cap._BCL && device->cap._BCM && max_level > 0) {
 		int result;
 		static int count = 0;
 		char *name;
@@ -753,7 +753,17 @@ static void acpi_video_device_find_cap(struct acpi_video_device *device)
 		device->backlight = backlight_device_register(name,
 			NULL, device, &acpi_backlight_ops);
 		device->backlight->props.max_brightness = device->brightness->count-3;
-		device->backlight->props.brightness = acpi_video_get_brightness(device->backlight);
+		/*
+		 * If there exists the _BQC object, the _BQC object will be
+		 * called to get the current backlight brightness. Otherwise
+		 * the brightness will be set to the maximum.
+		 */
+		if (device->cap._BQC)
+			device->backlight->props.brightness =
+				acpi_video_get_brightness(device->backlight);
+		else
+			device->backlight->props.brightness =
+				device->backlight->props.max_brightness;
 		backlight_update_status(device->backlight);
 		kfree(name);
 
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index dc7596f028b..ef3e5522e1a 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -1273,7 +1273,7 @@ static ssize_t ahci_transmit_led_message(struct ata_port *ap, u32 state,
 	void __iomem *mmio = ap->host->iomap[AHCI_PCI_BAR];
 	u32 em_ctl;
 	u32 message[] = {0, 0};
-	unsigned int flags;
+	unsigned long flags;
 	int pmp;
 	struct ahci_em_priv *emp;
 
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index d5c1bbfbe79..73338d231db 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -2562,7 +2562,8 @@ fore200e_load_and_start_fw(struct fore200e* fore200e)
     const struct firmware *firmware;
     struct device *device;
     struct fw_header *fw_header;
-    u32 *fw_data, fw_size;
+    const __le32 *fw_data;
+    u32 fw_size;
     u32 __iomem *load_addr;
     char buf[48];
     int err = -ENODEV;
@@ -2582,7 +2583,7 @@ fore200e_load_and_start_fw(struct fore200e* fore200e)
 	return err;
     }
 
-    fw_data = (u32 *) firmware->data;
+    fw_data = (__le32 *) firmware->data;
     fw_size = firmware->size / sizeof(u32);
     fw_header = (struct fw_header *) firmware->data;
     load_addr = fore200e->virt_base + le32_to_cpu(fw_header->load_offset);
@@ -3199,6 +3200,14 @@ static const struct fore200e_bus fore200e_bus[] = {
     {}
 };
 
-#ifdef MODULE_LICENSE
 MODULE_LICENSE("GPL");
+#ifdef CONFIG_PCI
+#ifdef __LITTLE_ENDIAN__
+MODULE_FIRMWARE("pca200e.bin");
+#else
+MODULE_FIRMWARE("pca200e_ecd.bin2");
+#endif
+#endif /* CONFIG_PCI */
+#ifdef CONFIG_SBUS
+MODULE_FIRMWARE("sba200e_ecd.bin2");
 #endif
diff --git a/drivers/auxdisplay/cfag12864b.c b/drivers/auxdisplay/cfag12864b.c
index 683509f013a..eacb175f6bd 100644
--- a/drivers/auxdisplay/cfag12864b.c
+++ b/drivers/auxdisplay/cfag12864b.c
@@ -336,16 +336,9 @@ static int __init cfag12864b_init(void)
 			"ks0108 is not initialized\n");
 		goto none;
 	}
+	BUILD_BUG_ON(PAGE_SIZE < CFAG12864B_SIZE);
 
-	if (PAGE_SIZE < CFAG12864B_SIZE) {
-		printk(KERN_ERR CFAG12864B_NAME ": ERROR: "
-			"page size (%i) < cfag12864b size (%i)\n",
-			(unsigned int)PAGE_SIZE, CFAG12864B_SIZE);
-		ret = -ENOMEM;
-		goto none;
-	}
-
-	cfag12864b_buffer = (unsigned char *) __get_free_page(GFP_KERNEL);
+	cfag12864b_buffer = (unsigned char *) get_zeroed_page(GFP_KERNEL);
 	if (cfag12864b_buffer == NULL) {
 		printk(KERN_ERR CFAG12864B_NAME ": ERROR: "
 			"can't get a free page\n");
@@ -367,8 +360,6 @@ static int __init cfag12864b_init(void)
 	if (cfag12864b_workqueue == NULL)
 		goto cachealloced;
 
-	memset(cfag12864b_buffer, 0, CFAG12864B_SIZE);
-
 	cfag12864b_clear();
 	cfag12864b_on();
 
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 7d5c63c81a5..068aa1c9538 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -116,12 +116,10 @@ static void device_release(struct kobject *kobj)
 		dev->type->release(dev);
 	else if (dev->class && dev->class->dev_release)
 		dev->class->dev_release(dev);
-	else {
-		printk(KERN_ERR "Device '%s' does not have a release() "
+	else
+		WARN(1, KERN_ERR "Device '%s' does not have a release() "
 			"function, it is broken and must be fixed.\n",
 			dev->bus_id);
-		WARN_ON(1);
-	}
 }
 
 static struct kobj_type device_ktype = {
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index b0be1d18fee..c9c92b00fd5 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -184,7 +184,7 @@ firmware_data_read(struct kobject *kobj, struct bin_attribute *bin_attr,
 	struct device *dev = to_dev(kobj);
 	struct firmware_priv *fw_priv = dev_get_drvdata(dev);
 	struct firmware *fw;
-	ssize_t ret_count = count;
+	ssize_t ret_count;
 
 	mutex_lock(&fw_lock);
 	fw = fw_priv->fw;
@@ -192,14 +192,8 @@ firmware_data_read(struct kobject *kobj, struct bin_attribute *bin_attr,
 		ret_count = -ENODEV;
 		goto out;
 	}
-	if (offset > fw->size) {
-		ret_count = 0;
-		goto out;
-	}
-	if (offset + ret_count > fw->size)
-		ret_count = fw->size - offset;
-
-	memcpy(buffer, fw->data + offset, ret_count);
+	ret_count = memory_read_from_buffer(buffer, count, &offset,
+						fw->data, fw->size);
 out:
 	mutex_unlock(&fw_lock);
 	return ret_count;
diff --git a/drivers/base/isa.c b/drivers/base/isa.c
index d2222397a40..efd57757494 100644
--- a/drivers/base/isa.c
+++ b/drivers/base/isa.c
@@ -7,6 +7,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/init.h>
+#include <linux/dma-mapping.h>
 #include <linux/isa.h>
 
 static struct device isa_bus = {
@@ -141,6 +142,9 @@ int isa_register_driver(struct isa_driver *isa_driver, unsigned int ndev)
 		isa_dev->dev.release		= isa_dev_release;
 		isa_dev->id			= id;
 
+		isa_dev->dev.coherent_dma_mask = DMA_24BIT_MASK;
+		isa_dev->dev.dma_mask = &isa_dev->dev.coherent_dma_mask;
+
 		error = device_register(&isa_dev->dev);
 		if (error) {
 			put_device(&isa_dev->dev);
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 4d4e0e7b6e9..3ad49a00029 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -101,6 +101,21 @@ static ssize_t show_mem_phys_index(struct sys_device *dev,
 }
 
 /*
+ * Show whether the section of memory is likely to be hot-removable
+ */
+static ssize_t show_mem_removable(struct sys_device *dev, char *buf)
+{
+	unsigned long start_pfn;
+	int ret;
+	struct memory_block *mem =
+		container_of(dev, struct memory_block, sysdev);
+
+	start_pfn = section_nr_to_pfn(mem->phys_index);
+	ret = is_mem_section_removable(start_pfn, PAGES_PER_SECTION);
+	return sprintf(buf, "%d\n", ret);
+}
+
+/*
  * online, offline, going offline, etc.
  */
 static ssize_t show_mem_state(struct sys_device *dev,
@@ -189,9 +204,8 @@ memory_block_action(struct memory_block *mem, unsigned long action)
 			}
 			break;
 		default:
-			printk(KERN_WARNING "%s(%p, %ld) unknown action: %ld\n",
+			WARN(1, KERN_WARNING "%s(%p, %ld) unknown action: %ld\n",
 					__func__, mem, action, action);
-			WARN_ON(1);
 			ret = -EINVAL;
 	}
 
@@ -262,6 +276,7 @@ static ssize_t show_phys_device(struct sys_device *dev,
 static SYSDEV_ATTR(phys_index, 0444, show_mem_phys_index, NULL);
 static SYSDEV_ATTR(state, 0644, show_mem_state, store_mem_state);
 static SYSDEV_ATTR(phys_device, 0444, show_phys_device, NULL);
+static SYSDEV_ATTR(removable, 0444, show_mem_removable, NULL);
 
 #define mem_create_simple_file(mem, attr_name)	\
 	sysdev_create_file(&mem->sysdev, &attr_##attr_name)
@@ -350,6 +365,8 @@ static int add_memory_block(unsigned long node_id, struct mem_section *section,
 		ret = mem_create_simple_file(mem, state);
 	if (!ret)
 		ret = mem_create_simple_file(mem, phys_device);
+	if (!ret)
+		ret = mem_create_simple_file(mem, removable);
 
 	return ret;
 }
@@ -394,6 +411,7 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
 	mem_remove_simple_file(mem, phys_index);
 	mem_remove_simple_file(mem, state);
 	mem_remove_simple_file(mem, phys_device);
+	mem_remove_simple_file(mem, removable);
 	unregister_memory(mem, section);
 
 	return 0;
diff --git a/drivers/base/sys.c b/drivers/base/sys.c
index 40fc14f0354..75dd6e22faf 100644
--- a/drivers/base/sys.c
+++ b/drivers/base/sys.c
@@ -168,19 +168,16 @@ int sysdev_driver_register(struct sysdev_class *cls, struct sysdev_driver *drv)
 	int err = 0;
 
 	if (!cls) {
-		printk(KERN_WARNING "sysdev: invalid class passed to "
+		WARN(1, KERN_WARNING "sysdev: invalid class passed to "
 			"sysdev_driver_register!\n");
-		WARN_ON(1);
 		return -EINVAL;
 	}
 
 	/* Check whether this driver has already been added to a class. */
-	if (drv->entry.next && !list_empty(&drv->entry)) {
-		printk(KERN_WARNING "sysdev: class %s: driver (%p) has already"
+	if (drv->entry.next && !list_empty(&drv->entry))
+		WARN(1, KERN_WARNING "sysdev: class %s: driver (%p) has already"
 			" been registered to a class, something is wrong, but "
 			"will forge on!\n", cls->name, drv);
-		WARN_ON(1);
-	}
 
 	mutex_lock(&sysdev_drivers_lock);
 	if (cls && kset_get(&cls->kset)) {
@@ -194,8 +191,7 @@ int sysdev_driver_register(struct sysdev_class *cls, struct sysdev_driver *drv)
 		}
 	} else {
 		err = -EINVAL;
-		printk(KERN_ERR "%s: invalid device class\n", __func__);
-		WARN_ON(1);
+		WARN(1, KERN_ERR "%s: invalid device class\n", __func__);
 	}
 	mutex_unlock(&sysdev_drivers_lock);
 	return err;
diff --git a/drivers/block/aoe/aoechr.c b/drivers/block/aoe/aoechr.c
index c04440cd6a3..181ebb85f0b 100644
--- a/drivers/block/aoe/aoechr.c
+++ b/drivers/block/aoe/aoechr.c
@@ -6,6 +6,7 @@
 
 #include <linux/hdreg.h>
 #include <linux/blkdev.h>
+#include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/smp_lock.h>
 #include "aoe.h"
@@ -36,7 +37,7 @@ struct ErrMsg {
 
 static struct ErrMsg emsgs[NMSG];
 static int emsgs_head_idx, emsgs_tail_idx;
-static struct semaphore emsgs_sema;
+static struct completion emsgs_comp;
 static spinlock_t emsgs_lock;
 static int nblocked_emsgs_readers;
 static struct class *aoe_class;
@@ -141,7 +142,7 @@ bail:		spin_unlock_irqrestore(&emsgs_lock, flags);
 	spin_unlock_irqrestore(&emsgs_lock, flags);
 
 	if (nblocked_emsgs_readers)
-		up(&emsgs_sema);
+		complete(&emsgs_comp);
 }
 
 static ssize_t
@@ -221,7 +222,7 @@ aoechr_read(struct file *filp, char __user *buf, size_t cnt, loff_t *off)
 
 		spin_unlock_irqrestore(&emsgs_lock, flags);
 
-		n = down_interruptible(&emsgs_sema);
+		n = wait_for_completion_interruptible(&emsgs_comp);
 
 		spin_lock_irqsave(&emsgs_lock, flags);
 
@@ -269,7 +270,7 @@ aoechr_init(void)
 		printk(KERN_ERR "aoe: can't register char device\n");
 		return n;
 	}
-	sema_init(&emsgs_sema, 0);
+	init_completion(&emsgs_comp);
 	spin_lock_init(&emsgs_lock);
 	aoe_class = class_create(THIS_MODULE, "aoe");
 	if (IS_ERR(aoe_class)) {
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index dd7ea203f94..42251095134 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -196,6 +196,7 @@ static int virtblk_probe(struct virtio_device *vdev)
 	int err;
 	u64 cap;
 	u32 v;
+	u32 blk_size;
 
 	if (index_to_minor(index) >= 1 << MINORBITS)
 		return -ENOSPC;
@@ -290,6 +291,13 @@ static int virtblk_probe(struct virtio_device *vdev)
 	if (!err)
 		blk_queue_max_hw_segments(vblk->disk->queue, v);
 
+	/* Host can optionally specify the block size of the device */
+	err = virtio_config_val(vdev, VIRTIO_BLK_F_BLK_SIZE,
+				offsetof(struct virtio_blk_config, blk_size),
+				&blk_size);
+	if (!err)
+		blk_queue_hardsect_size(vblk->disk->queue, blk_size);
+
 	add_disk(vblk->disk);
 	return 0;
 
@@ -330,7 +338,7 @@ static struct virtio_device_id id_table[] = {
 
 static unsigned int features[] = {
 	VIRTIO_BLK_F_BARRIER, VIRTIO_BLK_F_SEG_MAX, VIRTIO_BLK_F_SIZE_MAX,
-	VIRTIO_BLK_F_GEOMETRY, VIRTIO_BLK_F_RO,
+	VIRTIO_BLK_F_GEOMETRY, VIRTIO_BLK_F_RO, VIRTIO_BLK_F_BLK_SIZE,
 };
 
 static struct virtio_driver virtio_blk = {
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index e0bbbfb6a36..d0ac944e169 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -36,6 +36,14 @@ config VT
 	  If unsure, say Y, or else you won't be able to do much with your new
 	  shiny Linux system :-)
 
+config CONSOLE_TRANSLATIONS
+	depends on VT
+	default y
+	bool "Enable character translations in console" if EMBEDDED
+	---help---
+	  This enables support for font mapping and Unicode translation
+	  on virtual consoles.
+
 config VT_CONSOLE
 	bool "Support for console on virtual terminal" if EMBEDDED
 	depends on VT
@@ -578,11 +586,14 @@ config HVC_DRIVER
 	  It will automatically be selected if one of the back-end console drivers
 	  is selected.
 
+config HVC_IRQ
+	bool
 
 config HVC_CONSOLE
 	bool "pSeries Hypervisor Virtual Console support"
 	depends on PPC_PSERIES
 	select HVC_DRIVER
+	select HVC_IRQ
 	help
 	  pSeries machines when partitioned support a hypervisor virtual
 	  console. This driver allows each pSeries partition to have a console
@@ -593,6 +604,7 @@ config HVC_ISERIES
 	depends on PPC_ISERIES
 	default y
 	select HVC_DRIVER
+	select HVC_IRQ
 	help
 	  iSeries machines support a hypervisor virtual console.
 
@@ -614,13 +626,18 @@ config HVC_XEN
 	bool "Xen Hypervisor Console support"
 	depends on XEN
 	select HVC_DRIVER
+	select HVC_IRQ
 	default y
 	help
 	  Xen virtual console device driver
 
 config VIRTIO_CONSOLE
-	bool
+	tristate "Virtio console"
+	depends on VIRTIO
 	select HVC_DRIVER
+	help
+	  Virtio console for use with lguest and other hypervisors.
+
 
 config HVCS
 	tristate "IBM Hypervisor Virtual Console Server support"
@@ -857,13 +874,6 @@ config DS1302
 
 endif # RTC_LIB
 
-config COBALT_LCD
-	bool "Support for Cobalt LCD"
-	depends on MIPS_COBALT
-	help
-	  This option enables support for the LCD display and buttons found
-	  on Cobalt systems through a misc device.
-
 config DTLK
 	tristate "Double Talk PC internal speech card support"
 	depends on ISA
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index dc5a327d72d..8a161c30e1d 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -12,8 +12,8 @@ obj-y	 += mem.o random.o tty_io.o n_tty.o tty_ioctl.o tty_ldisc.o
 obj-$(CONFIG_LEGACY_PTYS)	+= pty.o
 obj-$(CONFIG_UNIX98_PTYS)	+= pty.o
 obj-y				+= misc.o
-obj-$(CONFIG_VT)		+= vt_ioctl.o vc_screen.o consolemap.o \
-				   consolemap_deftbl.o selection.o keyboard.o
+obj-$(CONFIG_VT)		+= vt_ioctl.o vc_screen.o selection.o keyboard.o
+obj-$(CONFIG_CONSOLE_TRANSLATIONS) += consolemap.o consolemap_deftbl.o
 obj-$(CONFIG_HW_CONSOLE)	+= vt.o defkeymap.o
 obj-$(CONFIG_AUDIT)		+= tty_audit.o
 obj-$(CONFIG_MAGIC_SYSRQ)	+= sysrq.o
@@ -48,6 +48,7 @@ obj-$(CONFIG_HVC_ISERIES)	+= hvc_iseries.o
 obj-$(CONFIG_HVC_RTAS)		+= hvc_rtas.o
 obj-$(CONFIG_HVC_BEAT)		+= hvc_beat.o
 obj-$(CONFIG_HVC_DRIVER)	+= hvc_console.o
+obj-$(CONFIG_HVC_IRQ)		+= hvc_irq.o
 obj-$(CONFIG_HVC_XEN)		+= hvc_xen.o
 obj-$(CONFIG_VIRTIO_CONSOLE)	+= virtio_console.o
 obj-$(CONFIG_RAW_DRIVER)	+= raw.o
@@ -63,7 +64,6 @@ obj-$(CONFIG_BRIQ_PANEL)	+= briq_panel.o
 obj-$(CONFIG_BFIN_OTP)		+= bfin-otp.o
 
 obj-$(CONFIG_PRINTER)		+= lp.o
-obj-$(CONFIG_TIPAR)		+= tipar.o
 
 obj-$(CONFIG_APM_EMULATION)	+= apm-emulation.o
 
@@ -88,7 +88,6 @@ obj-$(CONFIG_TOSHIBA)		+= toshiba.o
 obj-$(CONFIG_I8K)		+= i8k.o
 obj-$(CONFIG_DS1620)		+= ds1620.o
 obj-$(CONFIG_HW_RANDOM)		+= hw_random/
-obj-$(CONFIG_COBALT_LCD)	+= lcd.o
 obj-$(CONFIG_PPDEV)		+= ppdev.o
 obj-$(CONFIG_NWBUTTON)		+= nwbutton.o
 obj-$(CONFIG_NWFLASH)		+= nwflash.o
diff --git a/drivers/char/ds1302.c b/drivers/char/ds1302.c
index fada6ddefba..c5e67a62395 100644
--- a/drivers/char/ds1302.c
+++ b/drivers/char/ds1302.c
@@ -20,10 +20,11 @@
 #include <linux/miscdevice.h>
 #include <linux/delay.h>
 #include <linux/bcd.h>
+#include <linux/smp_lock.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
 
-#include <asm/uaccess.h>
 #include <asm/system.h>
-#include <asm/io.h>
 #include <asm/rtc.h>
 #if defined(CONFIG_M32R)
 #include <asm/m32r.h>
@@ -153,9 +154,7 @@ static unsigned char days_in_mo[] =
 
 /* ioctl that supports RTC_RD_TIME and RTC_SET_TIME (read and set time/date). */
 
-static int
-rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
-	  unsigned long arg)
+static long rtc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	unsigned long flags;
 
@@ -165,7 +164,9 @@ rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			struct rtc_time rtc_tm;
 
 			memset(&rtc_tm, 0, sizeof (struct rtc_time));
+			lock_kernel();
 			get_rtc_time(&rtc_tm);
+			unlock_kernel();
 			if (copy_to_user((struct rtc_time*)arg, &rtc_tm, sizeof(struct rtc_time)))
 				return -EFAULT;
 			return 0;
@@ -217,6 +218,7 @@ rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			BIN_TO_BCD(mon);
 			BIN_TO_BCD(yrs);
 
+			lock_kernel();
 			local_irq_save(flags);
 			CMOS_WRITE(yrs, RTC_YEAR);
 			CMOS_WRITE(mon, RTC_MONTH);
@@ -225,6 +227,7 @@ rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			CMOS_WRITE(min, RTC_MINUTES);
 			CMOS_WRITE(sec, RTC_SECONDS);
 			local_irq_restore(flags);
+			unlock_kernel();
 
 			/* Notice that at this point, the RTC is updated but
 			 * the kernel is still running with the old time.
@@ -244,8 +247,10 @@ rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			if(copy_from_user(&tcs_val, (int*)arg, sizeof(int)))
 				return -EFAULT;
 
+			lock_kernel();
 			tcs_val = RTC_TCR_PATTERN | (tcs_val & 0x0F);
 			ds1302_writereg(RTC_TRICKLECHARGER, tcs_val);
+			unlock_kernel();
 			return 0;
 		}
 		default:
@@ -282,7 +287,7 @@ get_rtc_status(char *buf)
 
 static const struct file_operations rtc_fops = {
 	.owner		= THIS_MODULE,
-	.ioctl		= rtc_ioctl,
+	.unlocked_ioctl	= rtc_ioctl,
 };
 
 /* Probe for the chip by writing something to its RAM and try reading it back. */
diff --git a/drivers/char/dsp56k.c b/drivers/char/dsp56k.c
index 33c466a4888..ca7c72a486b 100644
--- a/drivers/char/dsp56k.c
+++ b/drivers/char/dsp56k.c
@@ -36,10 +36,10 @@
 #include <linux/smp_lock.h>
 #include <linux/firmware.h>
 #include <linux/platform_device.h>
+#include <linux/uaccess.h>	/* For put_user and get_user */
 
 #include <asm/atarihw.h>
 #include <asm/traps.h>
-#include <asm/uaccess.h>	/* For put_user and get_user */
 
 #include <asm/dsp56k.h>
 
@@ -303,10 +303,10 @@ static ssize_t dsp56k_write(struct file *file, const char __user *buf, size_t co
 	}
 }
 
-static int dsp56k_ioctl(struct inode *inode, struct file *file,
-			unsigned int cmd, unsigned long arg)
+static long dsp56k_ioctl(struct file *file, unsigned int cmd,
+			 unsigned long arg)
 {
-	int dev = iminor(inode) & 0x0f;
+	int dev = iminor(file->f_path.dentry->d_inode) & 0x0f;
 	void __user *argp = (void __user *)arg;
 
 	switch(dev)
@@ -331,8 +331,9 @@ static int dsp56k_ioctl(struct inode *inode, struct file *file,
 			if (len > DSP56K_MAX_BINARY_LENGTH) {
 				return -EINVAL;
 			}
-    
+			lock_kernel();
 			r = dsp56k_upload(bin, len);
+			unlock_kernel();
 			if (r < 0) {
 				return r;
 			}
@@ -342,12 +343,16 @@ static int dsp56k_ioctl(struct inode *inode, struct file *file,
 		case DSP56K_SET_TX_WSIZE:
 			if (arg > 4 || arg < 1)
 				return -EINVAL;
+			lock_kernel();
 			dsp56k.tx_wsize = (int) arg;
+			unlock_kernel();
 			break;
 		case DSP56K_SET_RX_WSIZE:
 			if (arg > 4 || arg < 1)
 				return -EINVAL;
+			lock_kernel();
 			dsp56k.rx_wsize = (int) arg;
+			unlock_kernel();
 			break;
 		case DSP56K_HOST_FLAGS:
 		{
@@ -359,6 +364,7 @@ static int dsp56k_ioctl(struct inode *inode, struct file *file,
 			if(get_user(out, &hf->out) < 0)
 				return -EFAULT;
 
+			lock_kernel();
 			if ((dir & 0x1) && (out & 0x1))
 				dsp56k_host_interface.icr |= DSP56K_ICR_HF0;
 			else if (dir & 0x1)
@@ -373,14 +379,16 @@ static int dsp56k_ioctl(struct inode *inode, struct file *file,
 			if (dsp56k_host_interface.icr & DSP56K_ICR_HF1) status |= 0x2;
 			if (dsp56k_host_interface.isr & DSP56K_ISR_HF2) status |= 0x4;
 			if (dsp56k_host_interface.isr & DSP56K_ISR_HF3) status |= 0x8;
-
+			unlock_kernel();
 			return put_user(status, &hf->status);
 		}
 		case DSP56K_HOST_CMD:
 			if (arg > 31 || arg < 0)
 				return -EINVAL;
+			lock_kernel();
 			dsp56k_host_interface.cvr = (u_char)((arg & DSP56K_CVR_HV_MASK) |
 							     DSP56K_CVR_HC);
+			unlock_kernel();
 			break;
 		default:
 			return -EINVAL;
@@ -472,7 +480,7 @@ static const struct file_operations dsp56k_fops = {
 	.owner		= THIS_MODULE,
 	.read		= dsp56k_read,
 	.write		= dsp56k_write,
-	.ioctl		= dsp56k_ioctl,
+	.unlocked_ioctl	= dsp56k_ioctl,
 	.open		= dsp56k_open,
 	.release	= dsp56k_release,
 };
diff --git a/drivers/char/efirtc.c b/drivers/char/efirtc.c
index d57ca3e4e53..67fbd7aab5d 100644
--- a/drivers/char/efirtc.c
+++ b/drivers/char/efirtc.c
@@ -37,8 +37,9 @@
 #include <linux/rtc.h>
 #include <linux/proc_fs.h>
 #include <linux/efi.h>
+#include <linux/smp_lock.h>
+#include <linux/uaccess.h>
 
-#include <asm/uaccess.h>
 #include <asm/system.h>
 
 #define EFI_RTC_VERSION		"0.4"
@@ -51,8 +52,8 @@
 
 static DEFINE_SPINLOCK(efi_rtc_lock);
 
-static int efi_rtc_ioctl(struct inode *inode, struct file *file,
-		     unsigned int cmd, unsigned long arg);
+static long efi_rtc_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg);
 
 #define is_leap(year) \
           ((year) % 4 == 0 && ((year) % 100 != 0 || (year) % 400 == 0))
@@ -146,9 +147,8 @@ convert_from_efi_time(efi_time_t *eft, struct rtc_time *wtime)
 	}
 }
 
-static int
-efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
-		     unsigned long arg)
+static long efi_rtc_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg)
 {
 
 	efi_status_t	status;
@@ -175,13 +175,13 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			return -EINVAL;
 
 		case RTC_RD_TIME:
-
+			lock_kernel();
 			spin_lock_irqsave(&efi_rtc_lock, flags);
 
 			status = efi.get_time(&eft, &cap);
 
 			spin_unlock_irqrestore(&efi_rtc_lock,flags);
-
+			unlock_kernel();
 			if (status != EFI_SUCCESS) {
 				/* should never happen */
 				printk(KERN_ERR "efitime: can't read time\n");
@@ -203,11 +203,13 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 
 			convert_to_efi_time(&wtime, &eft);
 
+			lock_kernel();
 			spin_lock_irqsave(&efi_rtc_lock, flags);
 
 			status = efi.set_time(&eft);
 
 			spin_unlock_irqrestore(&efi_rtc_lock,flags);
+			unlock_kernel();
 
 			return status == EFI_SUCCESS ? 0 : -EINVAL;
 
@@ -223,6 +225,7 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 
 			convert_to_efi_time(&wtime, &eft);
 
+			lock_kernel();
 			spin_lock_irqsave(&efi_rtc_lock, flags);
 			/*
 			 * XXX Fixme:
@@ -233,16 +236,19 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			status = efi.set_wakeup_time((efi_bool_t)enabled, &eft);
 
 			spin_unlock_irqrestore(&efi_rtc_lock,flags);
+			unlock_kernel();
 
 			return status == EFI_SUCCESS ? 0 : -EINVAL;
 
 		case RTC_WKALM_RD:
 
+			lock_kernel();
 			spin_lock_irqsave(&efi_rtc_lock, flags);
 
 			status = efi.get_wakeup_time((efi_bool_t *)&enabled, (efi_bool_t *)&pending, &eft);
 
 			spin_unlock_irqrestore(&efi_rtc_lock,flags);
+			unlock_kernel();
 
 			if (status != EFI_SUCCESS) return -EINVAL;
 
@@ -256,7 +262,7 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			return copy_to_user(&ewp->time, &wtime,
 					    sizeof(struct rtc_time)) ? -EFAULT : 0;
 	}
-	return -EINVAL;
+	return -ENOTTY;
 }
 
 /*
@@ -265,8 +271,7 @@ efi_rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
  *	up things on a close.
  */
 
-static int
-efi_rtc_open(struct inode *inode, struct file *file)
+static int efi_rtc_open(struct inode *inode, struct file *file)
 {
 	/*
 	 * nothing special to do here
@@ -277,8 +282,7 @@ efi_rtc_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static int
-efi_rtc_close(struct inode *inode, struct file *file)
+static int efi_rtc_close(struct inode *inode, struct file *file)
 {
 	return 0;
 }
@@ -289,13 +293,12 @@ efi_rtc_close(struct inode *inode, struct file *file)
 
 static const struct file_operations efi_rtc_fops = {
 	.owner		= THIS_MODULE,
-	.ioctl		= efi_rtc_ioctl,
+	.unlocked_ioctl	= efi_rtc_ioctl,
 	.open		= efi_rtc_open,
 	.release	= efi_rtc_close,
 };
 
-static struct miscdevice efi_rtc_dev=
-{
+static struct miscdevice efi_rtc_dev= {
 	EFI_RTC_MINOR,
 	"efirtc",
 	&efi_rtc_fops
diff --git a/drivers/char/hpet.c b/drivers/char/hpet.c
index fb0a85a1eb3..b3f5dbc6d88 100644
--- a/drivers/char/hpet.c
+++ b/drivers/char/hpet.c
@@ -623,6 +623,7 @@ static inline int hpet_tpcheck(struct hpet_task *tp)
 	return -ENXIO;
 }
 
+#if 0
 int hpet_unregister(struct hpet_task *tp)
 {
 	struct hpet_dev *devp;
@@ -652,6 +653,7 @@ int hpet_unregister(struct hpet_task *tp)
 
 	return 0;
 }
+#endif  /*  0  */
 
 static ctl_table hpet_table[] = {
 	{
diff --git a/drivers/char/hvc_console.c b/drivers/char/hvc_console.c
index 2f9759d625c..02aac104842 100644
--- a/drivers/char/hvc_console.c
+++ b/drivers/char/hvc_console.c
@@ -27,7 +27,6 @@
 #include <linux/init.h>
 #include <linux/kbd_kern.h>
 #include <linux/kernel.h>
-#include <linux/kref.h>
 #include <linux/kthread.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -75,23 +74,6 @@ static int hvc_init(void);
 static int sysrq_pressed;
 #endif
 
-struct hvc_struct {
-	spinlock_t lock;
-	int index;
-	struct tty_struct *tty;
-	unsigned int count;
-	int do_wakeup;
-	char *outbuf;
-	int outbuf_size;
-	int n_outbuf;
-	uint32_t vtermno;
-	struct hv_ops *ops;
-	int irq_requested;
-	int irq;
-	struct list_head next;
-	struct kref kref; /* ref count & hvc_struct lifetime */
-};
-
 /* dynamic list of hvc_struct instances */
 static LIST_HEAD(hvc_structs);
 
@@ -298,27 +280,15 @@ int hvc_instantiate(uint32_t vtermno, int index, struct hv_ops *ops)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(hvc_instantiate);
 
 /* Wake the sleeping khvcd */
-static void hvc_kick(void)
+void hvc_kick(void)
 {
 	hvc_kicked = 1;
 	wake_up_process(hvc_task);
 }
-
-static int hvc_poll(struct hvc_struct *hp);
-
-/*
- * NOTE: This API isn't used if the console adapter doesn't support interrupts.
- * In this case the console is poll driven.
- */
-static irqreturn_t hvc_handle_interrupt(int irq, void *dev_instance)
-{
-	/* if hvc_poll request a repoll, then kick the hvcd thread */
-	if (hvc_poll(dev_instance))
-		hvc_kick();
-	return IRQ_HANDLED;
-}
+EXPORT_SYMBOL_GPL(hvc_kick);
 
 static void hvc_unthrottle(struct tty_struct *tty)
 {
@@ -333,7 +303,6 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 {
 	struct hvc_struct *hp;
 	unsigned long flags;
-	int irq = 0;
 	int rc = 0;
 
 	/* Auto increments kref reference if found. */
@@ -352,18 +321,15 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 	tty->low_latency = 1; /* Makes flushes to ldisc synchronous. */
 
 	hp->tty = tty;
-	/* Save for request_irq outside of spin_lock. */
-	irq = hp->irq;
-	if (irq)
-		hp->irq_requested = 1;
+
+	if (hp->ops->notifier_add)
+		rc = hp->ops->notifier_add(hp, hp->data);
 
 	spin_unlock_irqrestore(&hp->lock, flags);
-	/* check error, fallback to non-irq */
-	if (irq)
-		rc = request_irq(irq, hvc_handle_interrupt, IRQF_DISABLED, "hvc_console", hp);
+
 
 	/*
-	 * If the request_irq() fails and we return an error.  The tty layer
+	 * If the notifier fails we return an error.  The tty layer
 	 * will call hvc_close() after a failed open but we don't want to clean
 	 * up there so we'll clean up here and clear out the previously set
 	 * tty fields and return the kref reference.
@@ -371,7 +337,6 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 	if (rc) {
 		spin_lock_irqsave(&hp->lock, flags);
 		hp->tty = NULL;
-		hp->irq_requested = 0;
 		spin_unlock_irqrestore(&hp->lock, flags);
 		tty->driver_data = NULL;
 		kref_put(&hp->kref, destroy_hvc_struct);
@@ -386,7 +351,6 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 static void hvc_close(struct tty_struct *tty, struct file * filp)
 {
 	struct hvc_struct *hp;
-	int irq = 0;
 	unsigned long flags;
 
 	if (tty_hung_up_p(filp))
@@ -404,9 +368,8 @@ static void hvc_close(struct tty_struct *tty, struct file * filp)
 	spin_lock_irqsave(&hp->lock, flags);
 
 	if (--hp->count == 0) {
-		if (hp->irq_requested)
-			irq = hp->irq;
-		hp->irq_requested = 0;
+		if (hp->ops->notifier_del)
+			hp->ops->notifier_del(hp, hp->data);
 
 		/* We are done with the tty pointer now. */
 		hp->tty = NULL;
@@ -418,10 +381,6 @@ static void hvc_close(struct tty_struct *tty, struct file * filp)
 		 * waking periodically to check chars_in_buffer().
 		 */
 		tty_wait_until_sent(tty, HVC_CLOSE_WAIT);
-
-		if (irq)
-			free_irq(irq, hp);
-
 	} else {
 		if (hp->count < 0)
 			printk(KERN_ERR "hvc_close %X: oops, count is %d\n",
@@ -436,7 +395,6 @@ static void hvc_hangup(struct tty_struct *tty)
 {
 	struct hvc_struct *hp = tty->driver_data;
 	unsigned long flags;
-	int irq = 0;
 	int temp_open_count;
 
 	if (!hp)
@@ -458,13 +416,12 @@ static void hvc_hangup(struct tty_struct *tty)
 	hp->count = 0;
 	hp->n_outbuf = 0;
 	hp->tty = NULL;
-	if (hp->irq_requested)
-		/* Saved for use outside of spin_lock. */
-		irq = hp->irq;
-	hp->irq_requested = 0;
+
+	if (hp->ops->notifier_del)
+			hp->ops->notifier_del(hp, hp->data);
+
 	spin_unlock_irqrestore(&hp->lock, flags);
-	if (irq)
-		free_irq(irq, hp);
+
 	while(temp_open_count) {
 		--temp_open_count;
 		kref_put(&hp->kref, destroy_hvc_struct);
@@ -575,7 +532,7 @@ static u32 timeout = MIN_TIMEOUT;
 #define HVC_POLL_READ	0x00000001
 #define HVC_POLL_WRITE	0x00000002
 
-static int hvc_poll(struct hvc_struct *hp)
+int hvc_poll(struct hvc_struct *hp)
 {
 	struct tty_struct *tty;
 	int i, n, poll_mask = 0;
@@ -602,10 +559,10 @@ static int hvc_poll(struct hvc_struct *hp)
 	if (test_bit(TTY_THROTTLED, &tty->flags))
 		goto throttled;
 
-	/* If we aren't interrupt driven and aren't throttled, we always
+	/* If we aren't notifier driven and aren't throttled, we always
 	 * request a reschedule
 	 */
-	if (hp->irq == 0)
+	if (!hp->irq_requested)
 		poll_mask |= HVC_POLL_READ;
 
 	/* Read data if any */
@@ -674,6 +631,7 @@ static int hvc_poll(struct hvc_struct *hp)
 
 	return poll_mask;
 }
+EXPORT_SYMBOL_GPL(hvc_poll);
 
 /*
  * This kthread is either polling or interrupt driven.  This is determined by
@@ -733,7 +691,7 @@ static const struct tty_operations hvc_ops = {
 	.chars_in_buffer = hvc_chars_in_buffer,
 };
 
-struct hvc_struct __devinit *hvc_alloc(uint32_t vtermno, int irq,
+struct hvc_struct __devinit *hvc_alloc(uint32_t vtermno, int data,
 					struct hv_ops *ops, int outbuf_size)
 {
 	struct hvc_struct *hp;
@@ -754,7 +712,7 @@ struct hvc_struct __devinit *hvc_alloc(uint32_t vtermno, int irq,
 	memset(hp, 0x00, sizeof(*hp));
 
 	hp->vtermno = vtermno;
-	hp->irq = irq;
+	hp->data = data;
 	hp->ops = ops;
 	hp->outbuf_size = outbuf_size;
 	hp->outbuf = &((char *)hp)[ALIGN(sizeof(*hp), sizeof(long))];
@@ -784,6 +742,7 @@ struct hvc_struct __devinit *hvc_alloc(uint32_t vtermno, int irq,
 
 	return hp;
 }
+EXPORT_SYMBOL_GPL(hvc_alloc);
 
 int __devexit hvc_remove(struct hvc_struct *hp)
 {
diff --git a/drivers/char/hvc_console.h b/drivers/char/hvc_console.h
index 42ffb17e15d..d9ce1091562 100644
--- a/drivers/char/hvc_console.h
+++ b/drivers/char/hvc_console.h
@@ -26,6 +26,7 @@
 
 #ifndef HVC_CONSOLE_H
 #define HVC_CONSOLE_H
+#include <linux/kref.h>
 
 /*
  * This is the max number of console adapters that can/will be found as
@@ -42,24 +43,50 @@
  */
 #define HVC_ALLOC_TTY_ADAPTERS	8
 
+struct hvc_struct {
+	spinlock_t lock;
+	int index;
+	struct tty_struct *tty;
+	unsigned int count;
+	int do_wakeup;
+	char *outbuf;
+	int outbuf_size;
+	int n_outbuf;
+	uint32_t vtermno;
+	struct hv_ops *ops;
+	int irq_requested;
+	int data;
+	struct list_head next;
+	struct kref kref; /* ref count & hvc_struct lifetime */
+};
 
 /* implemented by a low level driver */
 struct hv_ops {
 	int (*get_chars)(uint32_t vtermno, char *buf, int count);
 	int (*put_chars)(uint32_t vtermno, const char *buf, int count);
-};
 
-struct hvc_struct;
+	/* Callbacks for notification. Called in open and close */
+	int (*notifier_add)(struct hvc_struct *hp, int irq);
+	void (*notifier_del)(struct hvc_struct *hp, int irq);
+};
 
 /* Register a vterm and a slot index for use as a console (console_init) */
 extern int hvc_instantiate(uint32_t vtermno, int index, struct hv_ops *ops);
 
 /* register a vterm for hvc tty operation (module_init or hotplug add) */
-extern struct hvc_struct * __devinit hvc_alloc(uint32_t vtermno, int irq,
+extern struct hvc_struct * __devinit hvc_alloc(uint32_t vtermno, int data,
 				struct hv_ops *ops, int outbuf_size);
-/* remove a vterm from hvc tty operation (modele_exit or hotplug remove) */
+/* remove a vterm from hvc tty operation (module_exit or hotplug remove) */
 extern int __devexit hvc_remove(struct hvc_struct *hp);
 
+/* data available */
+int hvc_poll(struct hvc_struct *hp);
+void hvc_kick(void);
+
+/* default notifier for irq based notification */
+extern int notifier_add_irq(struct hvc_struct *hp, int data);
+extern void notifier_del_irq(struct hvc_struct *hp, int data);
+
 
 #if defined(CONFIG_XMON) && defined(CONFIG_SMP)
 #include <asm/xmon.h>
diff --git a/drivers/char/hvc_irq.c b/drivers/char/hvc_irq.c
new file mode 100644
index 00000000000..73a59cdb894
--- /dev/null
+++ b/drivers/char/hvc_irq.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright IBM Corp. 2001,2008
+ *
+ * This file contains the IRQ specific code for hvc_console
+ *
+ */
+
+#include <linux/interrupt.h>
+
+#include "hvc_console.h"
+
+static irqreturn_t hvc_handle_interrupt(int irq, void *dev_instance)
+{
+	/* if hvc_poll request a repoll, then kick the hvcd thread */
+	if (hvc_poll(dev_instance))
+		hvc_kick();
+	return IRQ_HANDLED;
+}
+
+/*
+ * For IRQ based systems these callbacks can be used
+ */
+int notifier_add_irq(struct hvc_struct *hp, int irq)
+{
+	int rc;
+
+	if (!irq) {
+		hp->irq_requested = 0;
+		return 0;
+	}
+	rc = request_irq(irq, hvc_handle_interrupt, IRQF_DISABLED,
+			   "hvc_console", hp);
+	if (!rc)
+		hp->irq_requested = 1;
+	return rc;
+}
+
+void notifier_del_irq(struct hvc_struct *hp, int irq)
+{
+	if (!irq)
+		return;
+	free_irq(irq, hp);
+	hp->irq_requested = 0;
+}
diff --git a/drivers/char/hvc_iseries.c b/drivers/char/hvc_iseries.c
index a08f8f981c1..b71c610fe5a 100644
--- a/drivers/char/hvc_iseries.c
+++ b/drivers/char/hvc_iseries.c
@@ -200,6 +200,8 @@ done:
 static struct hv_ops hvc_get_put_ops = {
 	.get_chars = get_chars,
 	.put_chars = put_chars,
+	.notifier_add = notifier_add_irq,
+	.notifier_del = notifier_del_irq,
 };
 
 static int __devinit hvc_vio_probe(struct vio_dev *vdev,
diff --git a/drivers/char/hvc_vio.c b/drivers/char/hvc_vio.c
index 79711aa4b41..93f3840c168 100644
--- a/drivers/char/hvc_vio.c
+++ b/drivers/char/hvc_vio.c
@@ -80,6 +80,8 @@ static int filtered_get_chars(uint32_t vtermno, char *buf, int count)
 static struct hv_ops hvc_get_put_ops = {
 	.get_chars = filtered_get_chars,
 	.put_chars = hvc_put_chars,
+	.notifier_add = notifier_add_irq,
+	.notifier_del = notifier_del_irq,
 };
 
 static int __devinit hvc_vio_probe(struct vio_dev *vdev,
diff --git a/drivers/char/hvc_xen.c b/drivers/char/hvc_xen.c
index db2ae421627..6b70aa66a58 100644
--- a/drivers/char/hvc_xen.c
+++ b/drivers/char/hvc_xen.c
@@ -100,6 +100,8 @@ static int read_console(uint32_t vtermno, char *buf, int len)
 static struct hv_ops hvc_ops = {
 	.get_chars = read_console,
 	.put_chars = write_console,
+	.notifier_add = notifier_add_irq,
+	.notifier_del = notifier_del_irq,
 };
 
 static int __init xen_init(void)
diff --git a/drivers/char/ip2/ip2main.c b/drivers/char/ip2/ip2main.c
index 9cb48fcd316..689f9dcd3b8 100644
--- a/drivers/char/ip2/ip2main.c
+++ b/drivers/char/ip2/ip2main.c
@@ -203,7 +203,7 @@ static int set_serial_info(i2ChanStrPtr, struct serial_struct __user *);
 
 static ssize_t ip2_ipl_read(struct file *, char __user *, size_t, loff_t *);
 static ssize_t ip2_ipl_write(struct file *, const char __user *, size_t, loff_t *);
-static int ip2_ipl_ioctl(struct inode *, struct file *, UINT, ULONG);
+static long ip2_ipl_ioctl(struct file *, UINT, ULONG);
 static int ip2_ipl_open(struct inode *, struct file *);
 
 static int DumpTraceBuffer(char __user *, int);
@@ -236,7 +236,7 @@ static const struct file_operations ip2_ipl = {
 	.owner		= THIS_MODULE,
 	.read		= ip2_ipl_read,
 	.write		= ip2_ipl_write,
-	.ioctl		= ip2_ipl_ioctl,
+	.unlocked_ioctl	= ip2_ipl_ioctl,
 	.open		= ip2_ipl_open,
 }; 
 
@@ -2845,10 +2845,10 @@ ip2_ipl_write(struct file *pFile, const char __user *pData, size_t count, loff_t
 /*                                                                            */
 /*                                                                            */
 /******************************************************************************/
-static int
-ip2_ipl_ioctl ( struct inode *pInode, struct file *pFile, UINT cmd, ULONG arg )
+static long
+ip2_ipl_ioctl (struct file *pFile, UINT cmd, ULONG arg )
 {
-	unsigned int iplminor = iminor(pInode);
+	unsigned int iplminor = iminor(pFile->f_path.dentry->d_inode);
 	int rc = 0;
 	void __user *argp = (void __user *)arg;
 	ULONG __user *pIndex = argp;
@@ -2859,6 +2859,8 @@ ip2_ipl_ioctl ( struct inode *pInode, struct file *pFile, UINT cmd, ULONG arg )
 	printk (KERN_DEBUG "IP2IPL: ioctl cmd %d, arg %ld\n", cmd, arg );
 #endif
 
+	lock_kernel();
+
 	switch ( iplminor ) {
 	case 0:	    // IPL device
 		rc = -EINVAL;
@@ -2919,6 +2921,7 @@ ip2_ipl_ioctl ( struct inode *pInode, struct file *pFile, UINT cmd, ULONG arg )
 		rc = -ENODEV;
 		break;
 	}
+	unlock_kernel();
 	return rc;
 }
 
diff --git a/drivers/char/lcd.c b/drivers/char/lcd.c
deleted file mode 100644
index 1c29b20e4f4..00000000000
--- a/drivers/char/lcd.c
+++ /dev/null
@@ -1,516 +0,0 @@
-/*
- * LCD, LED and Button interface for Cobalt
- *
- * 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.
- *
- * Copyright (C) 1996, 1997 by Andrew Bose
- *
- * Linux kernel version history:
- *       March 2001: Ported from 2.0.34  by Liam Davies
- *
- */
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/miscdevice.h>
-#include <linux/slab.h>
-#include <linux/ioport.h>
-#include <linux/fcntl.h>
-#include <linux/mc146818rtc.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/smp_lock.h>
-#include <linux/delay.h>
-
-#include <asm/io.h>
-#include <asm/uaccess.h>
-#include <asm/system.h>
-
-#include "lcd.h"
-
-static int lcd_ioctl(struct inode *inode, struct file *file,
-		     unsigned int cmd, unsigned long arg);
-
-static unsigned int lcd_present = 1;
-
-/* used in arch/mips/cobalt/reset.c */
-int led_state = 0;
-
-#if defined(CONFIG_TULIP) && 0
-
-#define MAX_INTERFACES	8
-static linkcheck_func_t linkcheck_callbacks[MAX_INTERFACES];
-static void *linkcheck_cookies[MAX_INTERFACES];
-
-int lcd_register_linkcheck_func(int iface_num, void *func, void *cookie)
-{
-	if (iface_num < 0 ||
-	    iface_num >= MAX_INTERFACES ||
-	    linkcheck_callbacks[iface_num] != NULL)
-		return -1;
-	linkcheck_callbacks[iface_num] = (linkcheck_func_t) func;
-	linkcheck_cookies[iface_num] = cookie;
-	return 0;
-}
-#endif
-
-static int lcd_ioctl(struct inode *inode, struct file *file,
-		     unsigned int cmd, unsigned long arg)
-{
-	struct lcd_display button_display;
-	unsigned long address, a;
-
-	switch (cmd) {
-	case LCD_On:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x0F);
-		break;
-
-	case LCD_Off:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x08);
-		break;
-
-	case LCD_Reset:
-		udelay(150);
-		LCDWriteInst(0x3F);
-		udelay(150);
-		LCDWriteInst(0x3F);
-		udelay(150);
-		LCDWriteInst(0x3F);
-		udelay(150);
-		LCDWriteInst(0x3F);
-		udelay(150);
-		LCDWriteInst(0x01);
-		udelay(150);
-		LCDWriteInst(0x06);
-		break;
-
-	case LCD_Clear:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x01);
-		break;
-
-	case LCD_Cursor_Left:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x10);
-		break;
-
-	case LCD_Cursor_Right:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x14);
-		break;
-
-	case LCD_Cursor_Off:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x0C);
-		break;
-
-	case LCD_Cursor_On:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x0F);
-		break;
-
-	case LCD_Blink_Off:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x0E);
-		break;
-
-	case LCD_Get_Cursor_Pos:{
-			struct lcd_display display;
-
-			udelay(150);
-			BusyCheck();
-			display.cursor_address = (LCDReadInst);
-			display.cursor_address =
-			    (display.cursor_address & 0x07F);
-			if (copy_to_user
-			    ((struct lcd_display *) arg, &display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			break;
-		}
-
-
-	case LCD_Set_Cursor_Pos:{
-			struct lcd_display display;
-
-			if (copy_from_user
-			    (&display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			a = (display.cursor_address | kLCD_Addr);
-
-			udelay(150);
-			BusyCheck();
-			LCDWriteInst(a);
-
-			break;
-		}
-
-	case LCD_Get_Cursor:{
-			struct lcd_display display;
-
-			udelay(150);
-			BusyCheck();
-			display.character = LCDReadData;
-
-			if (copy_to_user
-			    ((struct lcd_display *) arg, &display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-			udelay(150);
-			BusyCheck();
-			LCDWriteInst(0x10);
-
-			break;
-		}
-
-	case LCD_Set_Cursor:{
-			struct lcd_display display;
-
-			if (copy_from_user
-			    (&display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			udelay(150);
-			BusyCheck();
-			LCDWriteData(display.character);
-			udelay(150);
-			BusyCheck();
-			LCDWriteInst(0x10);
-
-			break;
-		}
-
-
-	case LCD_Disp_Left:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x18);
-		break;
-
-	case LCD_Disp_Right:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x1C);
-		break;
-
-	case LCD_Home:
-		udelay(150);
-		BusyCheck();
-		LCDWriteInst(0x02);
-		break;
-
-	case LCD_Write:{
-			struct lcd_display display;
-			unsigned int index;
-
-
-			if (copy_from_user
-			    (&display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			udelay(150);
-			BusyCheck();
-			LCDWriteInst(0x80);
-			udelay(150);
-			BusyCheck();
-
-			for (index = 0; index < (display.size1); index++) {
-				udelay(150);
-				BusyCheck();
-				LCDWriteData(display.line1[index]);
-				BusyCheck();
-			}
-
-			udelay(150);
-			BusyCheck();
-			LCDWriteInst(0xC0);
-			udelay(150);
-			BusyCheck();
-			for (index = 0; index < (display.size2); index++) {
-				udelay(150);
-				BusyCheck();
-				LCDWriteData(display.line2[index]);
-			}
-
-			break;
-		}
-
-	case LCD_Read:{
-			struct lcd_display display;
-
-			BusyCheck();
-			for (address = kDD_R00; address <= kDD_R01;
-			     address++) {
-				a = (address | kLCD_Addr);
-
-				udelay(150);
-				BusyCheck();
-				LCDWriteInst(a);
-				udelay(150);
-				BusyCheck();
-				display.line1[address] = LCDReadData;
-			}
-
-			display.line1[0x27] = '\0';
-
-			for (address = kDD_R10; address <= kDD_R11;
-			     address++) {
-				a = (address | kLCD_Addr);
-
-				udelay(150);
-				BusyCheck();
-				LCDWriteInst(a);
-
-				udelay(150);
-				BusyCheck();
-				display.line2[address - 0x40] =
-				    LCDReadData;
-			}
-
-			display.line2[0x27] = '\0';
-
-			if (copy_to_user
-			    ((struct lcd_display *) arg, &display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-			break;
-		}
-
-//  set all GPIO leds to led_display.leds
-
-	case LED_Set:{
-			struct lcd_display led_display;
-
-
-			if (copy_from_user
-			    (&led_display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			led_state = led_display.leds;
-			LEDSet(led_state);
-
-			break;
-		}
-
-
-//  set only bit led_display.leds
-
-	case LED_Bit_Set:{
-			unsigned int i;
-			int bit = 1;
-			struct lcd_display led_display;
-
-
-			if (copy_from_user
-			    (&led_display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			for (i = 0; i < (int) led_display.leds; i++) {
-				bit = 2 * bit;
-			}
-
-			led_state = led_state | bit;
-			LEDSet(led_state);
-			break;
-		}
-
-//  clear only bit led_display.leds
-
-	case LED_Bit_Clear:{
-			unsigned int i;
-			int bit = 1;
-			struct lcd_display led_display;
-
-
-			if (copy_from_user
-			    (&led_display, (struct lcd_display *) arg,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-
-			for (i = 0; i < (int) led_display.leds; i++) {
-				bit = 2 * bit;
-			}
-
-			led_state = led_state & ~bit;
-			LEDSet(led_state);
-			break;
-		}
-
-
-	case BUTTON_Read:{
-			button_display.buttons = GPIRead;
-			if (copy_to_user
-			    ((struct lcd_display *) arg, &button_display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-			break;
-		}
-
-	case LINK_Check:{
-			button_display.buttons =
-			    *((volatile unsigned long *) (0xB0100060));
-			if (copy_to_user
-			    ((struct lcd_display *) arg, &button_display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-			break;
-		}
-
-	case LINK_Check_2:{
-			int iface_num;
-
-			/* panel-utils should pass in the desired interface status is wanted for
-			 * in "buttons" of the structure.  We will set this to non-zero if the
-			 * link is in fact up for the requested interface.  --DaveM
-			 */
-			if (copy_from_user
-			    (&button_display, (struct lcd_display *) arg,
-			     sizeof(button_display)))
-				return -EFAULT;
-			iface_num = button_display.buttons;
-#if defined(CONFIG_TULIP) && 0
-			if (iface_num >= 0 &&
-			    iface_num < MAX_INTERFACES &&
-			    linkcheck_callbacks[iface_num] != NULL) {
-				button_display.buttons =
-				    linkcheck_callbacks[iface_num]
-				    (linkcheck_cookies[iface_num]);
-			} else
-#endif
-				button_display.buttons = 0;
-
-			if (__copy_to_user
-			    ((struct lcd_display *) arg, &button_display,
-			     sizeof(struct lcd_display)))
-				return -EFAULT;
-			break;
-		}
-
-	default:
-		return -EINVAL;
-
-	}
-
-	return 0;
-
-}
-
-static int lcd_open(struct inode *inode, struct file *file)
-{
-	cycle_kernel_lock();
-
-	if (!lcd_present)
-		return -ENXIO;
-	else
-		return 0;
-}
-
-/* Only RESET or NEXT counts as button pressed */
-
-static inline int button_pressed(void)
-{
-	unsigned long buttons = GPIRead;
-
-	if ((buttons == BUTTON_Next) || (buttons == BUTTON_Next_B)
-	    || (buttons == BUTTON_Reset_B))
-		return buttons;
-	return 0;
-}
-
-/* LED daemon sits on this and we wake him up once a key is pressed. */
-
-static int lcd_waiters = 0;
-
-static ssize_t lcd_read(struct file *file, char *buf,
-		     size_t count, loff_t *ofs)
-{
-	long buttons_now;
-
-	if (lcd_waiters > 0)
-		return -EINVAL;
-
-	lcd_waiters++;
-	while (((buttons_now = (long) button_pressed()) == 0) &&
-	       !(signal_pending(current))) {
-		msleep_interruptible(2000);
-	}
-	lcd_waiters--;
-
-	if (signal_pending(current))
-		return -ERESTARTSYS;
-	return buttons_now;
-}
-
-/*
- *	The various file operations we support.
- */
-
-static const struct file_operations lcd_fops = {
-	.read = lcd_read,
-	.ioctl = lcd_ioctl,
-	.open = lcd_open,
-};
-
-static struct miscdevice lcd_dev = {
-	MISC_DYNAMIC_MINOR,
-	"lcd",
-	&lcd_fops
-};
-
-static int lcd_init(void)
-{
-	int ret;
-	unsigned long data;
-
-	pr_info("%s\n", LCD_DRIVER);
-	ret = misc_register(&lcd_dev);
-	if (ret) {
-		printk(KERN_WARNING LCD "Unable to register misc device.\n");
-		return ret;
-	}
-
-	/* Check region? Naaah! Just snarf it up. */
-/*	request_region(RTC_PORT(0), RTC_IO_EXTENT, "lcd");*/
-
-	udelay(150);
-	data = LCDReadData;
-	if ((data & 0x000000FF) == (0x00)) {
-		lcd_present = 0;
-		pr_info(LCD "LCD Not Present\n");
-	} else {
-		lcd_present = 1;
-		WRITE_GAL(kGal_DevBank2PReg, kGal_DevBank2Cfg);
-		WRITE_GAL(kGal_DevBank3PReg, kGal_DevBank3Cfg);
-	}
-
-	return 0;
-}
-
-static void __exit lcd_exit(void)
-{
-	misc_deregister(&lcd_dev);
-}
-
-module_init(lcd_init);
-module_exit(lcd_exit);
-
-MODULE_AUTHOR("Andrew Bose");
-MODULE_LICENSE("GPL");
diff --git a/drivers/char/lcd.h b/drivers/char/lcd.h
deleted file mode 100644
index 290b3ff23b0..00000000000
--- a/drivers/char/lcd.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- * LED, LCD and Button panel driver for Cobalt
- *
- * 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.
- *
- * Copyright (C) 1996, 1997 by Andrew Bose
- *
- * Linux kernel version history:
- *       March 2001: Ported from 2.0.34  by Liam Davies
- *
- */
-
-// function headers
-
-#define LCD_CHARS_PER_LINE 40
-#define MAX_IDLE_TIME 120
-
-struct lcd_display {
-        unsigned buttons;
-        int size1;
-        int size2;
-        unsigned char line1[LCD_CHARS_PER_LINE];
-        unsigned char line2[LCD_CHARS_PER_LINE];
-        unsigned char cursor_address;
-        unsigned char character;
-        unsigned char leds;
-        unsigned char *RomImage;
-};
-
-
-
-#define LCD_DRIVER	"Cobalt LCD Driver v2.10"
-
-#define LCD		"lcd: "
-
-#define kLCD_IR		0x0F000000
-#define kLCD_DR		0x0F000010
-#define kGPI		0x0D000000
-#define kLED		0x0C000000
-
-#define kDD_R00         0x00
-#define kDD_R01         0x27
-#define kDD_R10         0x40
-#define kDD_R11         0x67
-
-#define kLCD_Addr       0x00000080
-
-#define LCDTimeoutValue	0xfff
-
-
-// Macros
-
-#define LCDWriteData(x)	outl((x << 24), kLCD_DR)
-#define LCDWriteInst(x)	outl((x << 24), kLCD_IR)
-
-#define LCDReadData	(inl(kLCD_DR) >> 24)
-#define LCDReadInst	(inl(kLCD_IR) >> 24)
-
-#define GPIRead		(inl(kGPI) >> 24)
-
-#define LEDSet(x)	outb((char)x, kLED)
-
-#define WRITE_GAL(x,y)	outl(y, 0x04000000 | (x))
-#define BusyCheck()	while ((LCDReadInst & 0x80) == 0x80)
-
-
-
-/*
- * Function command codes for io_ctl.
- */
-#define LCD_On			1
-#define LCD_Off			2
-#define LCD_Clear		3
-#define LCD_Reset		4
-#define LCD_Cursor_Left		5
-#define LCD_Cursor_Right	6
-#define LCD_Disp_Left		7
-#define LCD_Disp_Right		8
-#define LCD_Get_Cursor		9
-#define LCD_Set_Cursor		10
-#define LCD_Home		11
-#define LCD_Read		12
-#define LCD_Write		13
-#define LCD_Cursor_Off		14
-#define LCD_Cursor_On		15
-#define LCD_Get_Cursor_Pos	16
-#define LCD_Set_Cursor_Pos	17
-#define LCD_Blink_Off           18
-
-#define LED_Set			40
-#define LED_Bit_Set		41
-#define LED_Bit_Clear		42
-
-
-//  Button defs
-#define BUTTON_Read             50
-
-
-// Ethernet LINK check hackaroo
-#define LINK_Check              90
-#define LINK_Check_2		91
-
-//  Button patterns  _B - single layer lcd boards
-
-#define BUTTON_NONE               0x3F
-#define BUTTON_NONE_B             0xFE
-
-#define BUTTON_Left               0x3B
-#define BUTTON_Left_B             0xFA
-
-#define BUTTON_Right              0x37
-#define BUTTON_Right_B            0xDE
-
-#define BUTTON_Up                 0x2F
-#define BUTTON_Up_B               0xF6
-
-#define BUTTON_Down               0x1F
-#define BUTTON_Down_B             0xEE
-
-#define BUTTON_Next               0x3D
-#define BUTTON_Next_B             0x7E
-
-#define BUTTON_Enter              0x3E
-#define BUTTON_Enter_B            0xBE
-
-#define BUTTON_Reset_B            0xFC
-
-
-// debounce constants
-
-#define BUTTON_SENSE            160000
-#define BUTTON_DEBOUNCE		5000
-
-
-//  Galileo register stuff
-
-#define kGal_DevBank2Cfg        0x1466DB33
-#define kGal_DevBank2PReg       0x464
-#define kGal_DevBank3Cfg        0x146FDFFB
-#define kGal_DevBank3PReg       0x468
-
-// Network
-
-#define kIPADDR			1
-#define kNETMASK		2
-#define kGATEWAY		3
-#define kDNS			4
-
-#define kClassA			5
-#define kClassB			6
-#define kClassC			7
-
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index c2dba82eb5f..672b08e694d 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -327,7 +327,10 @@ static void mmap_mem_close(struct vm_area_struct *vma)
 
 static struct vm_operations_struct mmap_mem_ops = {
 	.open  = mmap_mem_open,
-	.close = mmap_mem_close
+	.close = mmap_mem_close,
+#ifdef CONFIG_HAVE_IOREMAP_PROT
+	.access = generic_access_phys
+#endif
 };
 
 static int mmap_mem(struct file * file, struct vm_area_struct * vma)
diff --git a/drivers/char/mspec.c b/drivers/char/mspec.c
index fe2a95b5d3c..30f095a8c2d 100644
--- a/drivers/char/mspec.c
+++ b/drivers/char/mspec.c
@@ -193,25 +193,23 @@ mspec_close(struct vm_area_struct *vma)
 }
 
 /*
- * mspec_nopfn
+ * mspec_fault
  *
  * Creates a mspec page and maps it to user space.
  */
-static unsigned long
-mspec_nopfn(struct vm_area_struct *vma, unsigned long address)
+static int
+mspec_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 {
 	unsigned long paddr, maddr;
 	unsigned long pfn;
-	int index;
+	pgoff_t index = vmf->pgoff;
 	struct vma_data *vdata = vma->vm_private_data;
 
-	BUG_ON(address < vdata->vm_start || address >= vdata->vm_end);
-	index = (address - vdata->vm_start) >> PAGE_SHIFT;
 	maddr = (volatile unsigned long) vdata->maddr[index];
 	if (maddr == 0) {
 		maddr = uncached_alloc_page(numa_node_id(), 1);
 		if (maddr == 0)
-			return NOPFN_OOM;
+			return VM_FAULT_OOM;
 
 		spin_lock(&vdata->lock);
 		if (vdata->maddr[index] == 0) {
@@ -231,13 +229,20 @@ mspec_nopfn(struct vm_area_struct *vma, unsigned long address)
 
 	pfn = paddr >> PAGE_SHIFT;
 
-	return pfn;
+	/*
+	 * vm_insert_pfn can fail with -EBUSY, but in that case it will
+	 * be because another thread has installed the pte first, so it
+	 * is no problem.
+	 */
+	vm_insert_pfn(vma, (unsigned long)vmf->virtual_address, pfn);
+
+	return VM_FAULT_NOPAGE;
 }
 
 static struct vm_operations_struct mspec_vm_ops = {
 	.open = mspec_open,
 	.close = mspec_close,
-	.nopfn = mspec_nopfn
+	.fault = mspec_fault,
 };
 
 /*
diff --git a/drivers/char/mwave/mwavedd.c b/drivers/char/mwave/mwavedd.c
index 50243fcd87e..4f8d67fed29 100644
--- a/drivers/char/mwave/mwavedd.c
+++ b/drivers/char/mwave/mwavedd.c
@@ -86,8 +86,8 @@ module_param(mwave_uart_io, int, 0);
 
 static int mwave_open(struct inode *inode, struct file *file);
 static int mwave_close(struct inode *inode, struct file *file);
-static int mwave_ioctl(struct inode *inode, struct file *filp,
-                       unsigned int iocmd, unsigned long ioarg);
+static long mwave_ioctl(struct file *filp, unsigned int iocmd,
+							unsigned long ioarg);
 
 MWAVE_DEVICE_DATA mwave_s_mdd;
 
@@ -119,16 +119,16 @@ static int mwave_close(struct inode *inode, struct file *file)
 	return retval;
 }
 
-static int mwave_ioctl(struct inode *inode, struct file *file,
-                       unsigned int iocmd, unsigned long ioarg)
+static long mwave_ioctl(struct file *file, unsigned int iocmd,
+							unsigned long ioarg)
 {
 	unsigned int retval = 0;
 	pMWAVE_DEVICE_DATA pDrvData = &mwave_s_mdd;
 	void __user *arg = (void __user *)ioarg;
 
-	PRINTK_5(TRACE_MWAVE,
-		"mwavedd::mwave_ioctl, entry inode %p file %p cmd %x arg %x\n",
-		 inode,  file, iocmd, (int) ioarg);
+	PRINTK_4(TRACE_MWAVE,
+		"mwavedd::mwave_ioctl, entry file %p cmd %x arg %x\n",
+		file, iocmd, (int) ioarg);
 
 	switch (iocmd) {
 
@@ -136,7 +136,9 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 			PRINTK_1(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl, IOCTL_MW_RESET"
 				" calling tp3780I_ResetDSP\n");
+			lock_kernel();
 			retval = tp3780I_ResetDSP(&pDrvData->rBDData);
+			unlock_kernel();
 			PRINTK_2(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl, IOCTL_MW_RESET"
 				" retval %x from tp3780I_ResetDSP\n",
@@ -147,7 +149,9 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 			PRINTK_1(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl, IOCTL_MW_RUN"
 				" calling tp3780I_StartDSP\n");
+			lock_kernel();
 			retval = tp3780I_StartDSP(&pDrvData->rBDData);
+			unlock_kernel();
 			PRINTK_2(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl, IOCTL_MW_RUN"
 				" retval %x from tp3780I_StartDSP\n",
@@ -161,8 +165,10 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				"mwavedd::mwave_ioctl,"
 				" IOCTL_MW_DSP_ABILITIES calling"
 				" tp3780I_QueryAbilities\n");
+			lock_kernel();
 			retval = tp3780I_QueryAbilities(&pDrvData->rBDData,
 					&rAbilities);
+			unlock_kernel();
 			PRINTK_2(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl, IOCTL_MW_DSP_ABILITIES"
 				" retval %x from tp3780I_QueryAbilities\n",
@@ -193,11 +199,13 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				"mwavedd::mwave_ioctl IOCTL_MW_READ_DATA,"
 				" size %lx, ioarg %lx pusBuffer %p\n",
 				rReadData.ulDataLength, ioarg, pusBuffer);
+			lock_kernel();
 			retval = tp3780I_ReadWriteDspDStore(&pDrvData->rBDData,
 					iocmd,
 					pusBuffer,
 					rReadData.ulDataLength,
 					rReadData.usDspAddress);
+			unlock_kernel();
 		}
 			break;
 	
@@ -215,10 +223,12 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				" size %lx, ioarg %lx pusBuffer %p\n",
 				rReadData.ulDataLength / 2, ioarg,
 				pusBuffer);
+			lock_kernel();
 			retval = tp3780I_ReadWriteDspDStore(&pDrvData->rBDData,
 				iocmd, pusBuffer,
 				rReadData.ulDataLength / 2,
 				rReadData.usDspAddress);
+			unlock_kernel();
 		}
 			break;
 	
@@ -236,10 +246,12 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				" size %lx, ioarg %lx pusBuffer %p\n",
 				rWriteData.ulDataLength, ioarg,
 				pusBuffer);
+			lock_kernel();
 			retval = tp3780I_ReadWriteDspDStore(&pDrvData->rBDData,
 					iocmd, pusBuffer,
 					rWriteData.ulDataLength,
 					rWriteData.usDspAddress);
+			unlock_kernel();
 		}
 			break;
 	
@@ -257,10 +269,12 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				" size %lx, ioarg %lx pusBuffer %p\n",
 				rWriteData.ulDataLength, ioarg,
 				pusBuffer);
+			lock_kernel();
 			retval = tp3780I_ReadWriteDspIStore(&pDrvData->rBDData,
 					iocmd, pusBuffer,
 					rWriteData.ulDataLength,
 					rWriteData.usDspAddress);
+			unlock_kernel();
 		}
 			break;
 	
@@ -281,8 +295,10 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 						ipcnum);
 				return -EINVAL;
 			}
+			lock_kernel();
 			pDrvData->IPCs[ipcnum].bIsHere = FALSE;
 			pDrvData->IPCs[ipcnum].bIsEnabled = TRUE;
+			unlock_kernel();
 	
 			PRINTK_2(TRACE_MWAVE,
 				"mwavedd::mwave_ioctl IOCTL_MW_REGISTER_IPC"
@@ -307,6 +323,7 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 				return -EINVAL;
 			}
 	
+			lock_kernel();
 			if (pDrvData->IPCs[ipcnum].bIsEnabled == TRUE) {
 				DECLARE_WAITQUEUE(wait, current);
 
@@ -347,6 +364,7 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 					" processing\n",
 					ipcnum);
 			}
+			unlock_kernel();
 		}
 			break;
 	
@@ -365,19 +383,18 @@ static int mwave_ioctl(struct inode *inode, struct file *file,
 						ipcnum);
 				return -EINVAL;
 			}
+			lock_kernel();
 			if (pDrvData->IPCs[ipcnum].bIsEnabled == TRUE) {
 				pDrvData->IPCs[ipcnum].bIsEnabled = FALSE;
 				if (pDrvData->IPCs[ipcnum].bIsHere == TRUE) {
 					wake_up_interruptible(&pDrvData->IPCs[ipcnum].ipc_wait_queue);
 				}
 			}
+			unlock_kernel();
 		}
 			break;
 	
 		default:
-			PRINTK_ERROR(KERN_ERR_MWAVE "mwavedd::mwave_ioctl:"
-					" Error: Unrecognized iocmd %x\n",
-					iocmd);
 			return -ENOTTY;
 			break;
 	} /* switch */
@@ -460,7 +477,7 @@ static const struct file_operations mwave_fops = {
 	.owner		= THIS_MODULE,
 	.read		= mwave_read,
 	.write		= mwave_write,
-	.ioctl		= mwave_ioctl,
+	.unlocked_ioctl	= mwave_ioctl,
 	.open		= mwave_open,
 	.release	= mwave_close
 };
diff --git a/drivers/char/mwave/mwavedd.h b/drivers/char/mwave/mwavedd.h
index 8eca61e0a19..7e0d530e2e0 100644
--- a/drivers/char/mwave/mwavedd.h
+++ b/drivers/char/mwave/mwavedd.h
@@ -147,4 +147,6 @@ typedef struct _MWAVE_DEVICE_DATA {
 
 } MWAVE_DEVICE_DATA, *pMWAVE_DEVICE_DATA;
 
+extern MWAVE_DEVICE_DATA mwave_s_mdd;
+
 #endif
diff --git a/drivers/char/mwave/tp3780i.c b/drivers/char/mwave/tp3780i.c
index f282976daaa..c6896970806 100644
--- a/drivers/char/mwave/tp3780i.c
+++ b/drivers/char/mwave/tp3780i.c
@@ -57,8 +57,6 @@
 #include "3780i.h"
 #include "mwavepub.h"
 
-extern MWAVE_DEVICE_DATA mwave_s_mdd;
-
 static unsigned short s_ausThinkpadIrqToField[16] =
 	{ 0xFFFF, 0xFFFF, 0xFFFF, 0x0001, 0x0002, 0x0003, 0xFFFF, 0x0004,
 	0xFFFF, 0xFFFF, 0x0005, 0x0006, 0xFFFF, 0xFFFF, 0xFFFF, 0x0007 };
diff --git a/drivers/char/mxser.c b/drivers/char/mxser.c
index 4c756bbba94..e30575e8764 100644
--- a/drivers/char/mxser.c
+++ b/drivers/char/mxser.c
@@ -16,7 +16,6 @@
  *	Fed through a cleanup, indent and remove of non 2.6 code by Alan Cox
  *	<alan@redhat.com>. The original 1.8 code is available on www.moxa.com.
  *	- Fixed x86_64 cleanness
- *	- Fixed sleep with spinlock held in mxser_send_break
  */
 
 #include <linux/module.h>
@@ -49,18 +48,12 @@
 
 #define	MXSER_VERSION	"2.0.4"		/* 1.12 */
 #define	MXSERMAJOR	 174
-#define	MXSERCUMAJOR	 175
 
 #define MXSER_BOARDS		4	/* Max. boards */
 #define MXSER_PORTS_PER_BOARD	8	/* Max. ports per board */
 #define MXSER_PORTS		(MXSER_BOARDS * MXSER_PORTS_PER_BOARD)
 #define MXSER_ISR_PASS_LIMIT	100
 
-#define	MXSER_ERR_IOADDR	-1
-#define	MXSER_ERR_IRQ		-2
-#define	MXSER_ERR_IRQ_CONFLIT	-3
-#define	MXSER_ERR_VECTOR	-4
-
 /*CheckIsMoxaMust return value*/
 #define MOXA_OTHER_UART		0x00
 #define MOXA_MUST_MU150_HWID	0x01
@@ -179,14 +172,15 @@ static struct pci_device_id mxser_pcibrds[] = {
 };
 MODULE_DEVICE_TABLE(pci, mxser_pcibrds);
 
-static int ioaddr[MXSER_BOARDS] = { 0, 0, 0, 0 };
+static unsigned long ioaddr[MXSER_BOARDS];
 static int ttymajor = MXSERMAJOR;
 
 /* Variables for insmod */
 
 MODULE_AUTHOR("Casper Yang");
 MODULE_DESCRIPTION("MOXA Smartio/Industio Family Multiport Board Device Driver");
-module_param_array(ioaddr, int, NULL, 0);
+module_param_array(ioaddr, ulong, NULL, 0);
+MODULE_PARM_DESC(ioaddr, "ISA io addresses to look for a moxa board");
 module_param(ttymajor, int, 0);
 MODULE_LICENSE("GPL");
 
@@ -196,7 +190,6 @@ struct mxser_log {
 	unsigned long txcnt[MXSER_PORTS];
 };
 
-
 struct mxser_mon {
 	unsigned long rxcnt;
 	unsigned long txcnt;
@@ -287,19 +280,9 @@ struct mxser_mstatus {
 	int dcd;
 };
 
-static struct mxser_mstatus GMStatus[MXSER_PORTS];
-
-static int mxserBoardCAP[MXSER_BOARDS] = {
-	0, 0, 0, 0
-	/*  0x180, 0x280, 0x200, 0x320 */
-};
-
 static struct mxser_board mxser_boards[MXSER_BOARDS];
 static struct tty_driver *mxvar_sdriver;
 static struct mxser_log mxvar_log;
-static int mxvar_diagflag;
-static unsigned char mxser_msr[MXSER_PORTS + 1];
-static struct mxser_mon_ext mon_data_ext;
 static int mxser_set_baud_method[MXSER_PORTS + 1];
 
 static void mxser_enable_must_enchance_mode(unsigned long baseio)
@@ -543,6 +526,7 @@ static void process_txrx_fifo(struct mxser_port *info)
 
 static unsigned char mxser_get_msr(int baseaddr, int mode, int port)
 {
+	static unsigned char mxser_msr[MXSER_PORTS + 1];
 	unsigned char status = 0;
 
 	status = inb(baseaddr + UART_MSR);
@@ -1319,13 +1303,9 @@ static void mxser_flush_chars(struct tty_struct *tty)
 	struct mxser_port *info = tty->driver_data;
 	unsigned long flags;
 
-	if (info->xmit_cnt <= 0 ||
-			tty->stopped ||
-			!info->port.xmit_buf ||
-			(tty->hw_stopped &&
-			 (info->type != PORT_16550A) &&
-			 (!info->board->chip_flag)
-			))
+	if (info->xmit_cnt <= 0 || tty->stopped || !info->port.xmit_buf ||
+			(tty->hw_stopped && info->type != PORT_16550A &&
+			 !info->board->chip_flag))
 		return;
 
 	spin_lock_irqsave(&info->slock, flags);
@@ -1343,9 +1323,7 @@ static int mxser_write_room(struct tty_struct *tty)
 	int ret;
 
 	ret = SERIAL_XMIT_SIZE - info->xmit_cnt - 1;
-	if (ret < 0)
-		ret = 0;
-	return ret;
+	return ret < 0 ? 0 : ret;
 }
 
 static int mxser_chars_in_buffer(struct tty_struct *tty)
@@ -1634,6 +1612,8 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 
 	switch (cmd) {
 	case MOXA_GET_MAJOR:
+		printk(KERN_WARNING "mxser: '%s' uses deprecated ioctl %x, fix "
+				"your userspace\n", current->comm, cmd);
 		return put_user(ttymajor, (int __user *)argp);
 
 	case MOXA_CHKPORTENABLE:
@@ -1651,62 +1631,60 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 			ret = -EFAULT;
 		unlock_kernel();
 		return ret;
-	case MOXA_GETMSTATUS:
+	case MOXA_GETMSTATUS: {
+		struct mxser_mstatus ms, __user *msu = argp;
 		lock_kernel();
 		for (i = 0; i < MXSER_BOARDS; i++)
 			for (j = 0; j < MXSER_PORTS_PER_BOARD; j++) {
 				port = &mxser_boards[i].ports[j];
+				memset(&ms, 0, sizeof(ms));
 
-				GMStatus[i].ri = 0;
-				if (!port->ioaddr) {
-					GMStatus[i].dcd = 0;
-					GMStatus[i].dsr = 0;
-					GMStatus[i].cts = 0;
-					continue;
-				}
+				if (!port->ioaddr)
+					goto copy;
 
 				if (!port->port.tty || !port->port.tty->termios)
-					GMStatus[i].cflag =
-						port->normal_termios.c_cflag;
+					ms.cflag = port->normal_termios.c_cflag;
 				else
-					GMStatus[i].cflag =
-						port->port.tty->termios->c_cflag;
+					ms.cflag = port->port.tty->termios->c_cflag;
 
 				status = inb(port->ioaddr + UART_MSR);
-				if (status & 0x80 /*UART_MSR_DCD */ )
-					GMStatus[i].dcd = 1;
-				else
-					GMStatus[i].dcd = 0;
-
-				if (status & 0x20 /*UART_MSR_DSR */ )
-					GMStatus[i].dsr = 1;
-				else
-					GMStatus[i].dsr = 0;
-
-
-				if (status & 0x10 /*UART_MSR_CTS */ )
-					GMStatus[i].cts = 1;
-				else
-					GMStatus[i].cts = 0;
+				if (status & UART_MSR_DCD)
+					ms.dcd = 1;
+				if (status & UART_MSR_DSR)
+					ms.dsr = 1;
+				if (status & UART_MSR_CTS)
+					ms.cts = 1;
+			copy:
+				if (copy_to_user(msu, &ms, sizeof(ms))) {
+					unlock_kernel();
+					return -EFAULT;
+				}
+				msu++;
 			}
 		unlock_kernel();
-		if (copy_to_user(argp, GMStatus,
-				sizeof(struct mxser_mstatus) * MXSER_PORTS))
-			return -EFAULT;
 		return 0;
+	}
 	case MOXA_ASPP_MON_EXT: {
-		int p, shiftbit;
-		unsigned long opmode;
-		unsigned cflag, iflag;
+		struct mxser_mon_ext *me; /* it's 2k, stack unfriendly */
+		unsigned int cflag, iflag, p;
+		u8 opmode;
+
+		me = kzalloc(sizeof(*me), GFP_KERNEL);
+		if (!me)
+			return -ENOMEM;
 
 		lock_kernel();
-		for (i = 0; i < MXSER_BOARDS; i++) {
-			for (j = 0; j < MXSER_PORTS_PER_BOARD; j++) {
+		for (i = 0, p = 0; i < MXSER_BOARDS; i++) {
+			for (j = 0; j < MXSER_PORTS_PER_BOARD; j++, p++) {
+				if (p >= ARRAY_SIZE(me->rx_cnt)) {
+					i = MXSER_BOARDS;
+					break;
+				}
 				port = &mxser_boards[i].ports[j];
 				if (!port->ioaddr)
 					continue;
 
-				status = mxser_get_msr(port->ioaddr, 0, i);
+				status = mxser_get_msr(port->ioaddr, 0, p);
 
 				if (status & UART_MSR_TERI)
 					port->icount.rng++;
@@ -1718,16 +1696,13 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 					port->icount.cts++;
 
 				port->mon_data.modem_status = status;
-				mon_data_ext.rx_cnt[i] = port->mon_data.rxcnt;
-				mon_data_ext.tx_cnt[i] = port->mon_data.txcnt;
-				mon_data_ext.up_rxcnt[i] =
-					port->mon_data.up_rxcnt;
-				mon_data_ext.up_txcnt[i] =
-					port->mon_data.up_txcnt;
-				mon_data_ext.modem_status[i] =
+				me->rx_cnt[p] = port->mon_data.rxcnt;
+				me->tx_cnt[p] = port->mon_data.txcnt;
+				me->up_rxcnt[p] = port->mon_data.up_rxcnt;
+				me->up_txcnt[p] = port->mon_data.up_txcnt;
+				me->modem_status[p] =
 					port->mon_data.modem_status;
-				mon_data_ext.baudrate[i] =
-					tty_get_baud_rate(port->port.tty);
+				me->baudrate[p] = tty_get_baud_rate(port->port.tty);
 
 				if (!port->port.tty || !port->port.tty->termios) {
 					cflag = port->normal_termios.c_cflag;
@@ -1737,40 +1712,31 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 					iflag = port->port.tty->termios->c_iflag;
 				}
 
-				mon_data_ext.databits[i] = cflag & CSIZE;
-
-				mon_data_ext.stopbits[i] = cflag & CSTOPB;
-
-				mon_data_ext.parity[i] =
-					cflag & (PARENB | PARODD | CMSPAR);
-
-				mon_data_ext.flowctrl[i] = 0x00;
+				me->databits[p] = cflag & CSIZE;
+				me->stopbits[p] = cflag & CSTOPB;
+				me->parity[p] = cflag & (PARENB | PARODD |
+						CMSPAR);
 
 				if (cflag & CRTSCTS)
-					mon_data_ext.flowctrl[i] |= 0x03;
+					me->flowctrl[p] |= 0x03;
 
 				if (iflag & (IXON | IXOFF))
-					mon_data_ext.flowctrl[i] |= 0x0C;
+					me->flowctrl[p] |= 0x0C;
 
 				if (port->type == PORT_16550A)
-					mon_data_ext.fifo[i] = 1;
-				else
-					mon_data_ext.fifo[i] = 0;
+					me->fifo[p] = 1;
 
-				p = i % 4;
-				shiftbit = p * 2;
-				opmode = inb(port->opmode_ioaddr) >> shiftbit;
+				opmode = inb(port->opmode_ioaddr) >>
+						((p % 4) * 2);
 				opmode &= OP_MODE_MASK;
-
-				mon_data_ext.iftype[i] = opmode;
-
+				me->iftype[p] = opmode;
 			}
 		}
 		unlock_kernel();
-		if (copy_to_user(argp, &mon_data_ext,
-					sizeof(mon_data_ext)))
-			return -EFAULT;
-		return 0;
+		if (copy_to_user(argp, me, sizeof(*me)))
+			ret = -EFAULT;
+		kfree(me);
+		return ret;
 	}
 	default:
 		return -ENOIOCTLCMD;
@@ -1804,7 +1770,6 @@ static int mxser_ioctl(struct tty_struct *tty, struct file *file,
 {
 	struct mxser_port *info = tty->driver_data;
 	struct async_icount cnow;
-	struct serial_icounter_struct __user *p_cuser;
 	unsigned long flags;
 	void __user *argp = (void __user *)arg;
 	int retval;
@@ -1884,30 +1849,26 @@ static int mxser_ioctl(struct tty_struct *tty, struct file *file,
 	 * NB: both 1->0 and 0->1 transitions are counted except for
 	 *     RI where only 0->1 is counted.
 	 */
-	case TIOCGICOUNT:
+	case TIOCGICOUNT: {
+		struct serial_icounter_struct icnt = { 0 };
 		spin_lock_irqsave(&info->slock, flags);
 		cnow = info->icount;
 		spin_unlock_irqrestore(&info->slock, flags);
-		p_cuser = argp;
-		if (put_user(cnow.frame, &p_cuser->frame))
-			return -EFAULT;
-		if (put_user(cnow.brk, &p_cuser->brk))
-			return -EFAULT;
-		if (put_user(cnow.overrun, &p_cuser->overrun))
-			return -EFAULT;
-		if (put_user(cnow.buf_overrun, &p_cuser->buf_overrun))
-			return -EFAULT;
-		if (put_user(cnow.parity, &p_cuser->parity))
-			return -EFAULT;
-		if (put_user(cnow.rx, &p_cuser->rx))
-			return -EFAULT;
-		if (put_user(cnow.tx, &p_cuser->tx))
-			return -EFAULT;
-		put_user(cnow.cts, &p_cuser->cts);
-		put_user(cnow.dsr, &p_cuser->dsr);
-		put_user(cnow.rng, &p_cuser->rng);
-		put_user(cnow.dcd, &p_cuser->dcd);
-		return 0;
+
+		icnt.frame = cnow.frame;
+		icnt.brk = cnow.brk;
+		icnt.overrun = cnow.overrun;
+		icnt.buf_overrun = cnow.buf_overrun;
+		icnt.parity = cnow.parity;
+		icnt.rx = cnow.rx;
+		icnt.tx = cnow.tx;
+		icnt.cts = cnow.cts;
+		icnt.dsr = cnow.dsr;
+		icnt.rng = cnow.rng;
+		icnt.dcd = cnow.dcd;
+
+		return copy_to_user(argp, &icnt, sizeof(icnt)) ? -EFAULT : 0;
+	}
 	case MOXA_HighSpeedOn:
 		return put_user(info->baud_base != 115200 ? 1 : 0, (int __user *)argp);
 	case MOXA_SDS_RSTICOUNTER:
@@ -2503,7 +2464,8 @@ static int __devinit mxser_initbrd(struct mxser_board *brd,
 	unsigned int i;
 	int retval;
 
-	printk(KERN_INFO "max. baud rate = %d bps.\n", brd->ports[0].max_baud);
+	printk(KERN_INFO "mxser: max. baud rate = %d bps\n",
+			brd->ports[0].max_baud);
 
 	for (i = 0; i < brd->info->nports; i++) {
 		info = &brd->ports[i];
@@ -2586,28 +2548,32 @@ static int __init mxser_get_ISA_conf(int cap, struct mxser_board *brd)
 		irq = regs[9] & 0xF000;
 		irq = irq | (irq >> 4);
 		if (irq != (regs[9] & 0xFF00))
-			return MXSER_ERR_IRQ_CONFLIT;
+			goto err_irqconflict;
 	} else if (brd->info->nports == 4) {
 		irq = regs[9] & 0xF000;
 		irq = irq | (irq >> 4);
 		irq = irq | (irq >> 8);
 		if (irq != regs[9])
-			return MXSER_ERR_IRQ_CONFLIT;
+			goto err_irqconflict;
 	} else if (brd->info->nports == 8) {
 		irq = regs[9] & 0xF000;
 		irq = irq | (irq >> 4);
 		irq = irq | (irq >> 8);
 		if ((irq != regs[9]) || (irq != regs[10]))
-			return MXSER_ERR_IRQ_CONFLIT;
+			goto err_irqconflict;
 	}
 
-	if (!irq)
-		return MXSER_ERR_IRQ;
+	if (!irq) {
+		printk(KERN_ERR "mxser: interrupt number unset\n");
+		return -EIO;
+	}
 	brd->irq = ((int)(irq & 0xF000) >> 12);
 	for (i = 0; i < 8; i++)
 		brd->ports[i].ioaddr = (int) regs[i + 1] & 0xFFF8;
-	if ((regs[12] & 0x80) == 0)
-		return MXSER_ERR_VECTOR;
+	if ((regs[12] & 0x80) == 0) {
+		printk(KERN_ERR "mxser: invalid interrupt vector\n");
+		return -EIO;
+	}
 	brd->vector = (int)regs[11];	/* interrupt vector */
 	if (id == 1)
 		brd->vector_mask = 0x00FF;
@@ -2634,13 +2600,26 @@ static int __init mxser_get_ISA_conf(int cap, struct mxser_board *brd)
 	else
 		brd->uart_type = PORT_16450;
 	if (!request_region(brd->ports[0].ioaddr, 8 * brd->info->nports,
-			"mxser(IO)"))
-		return MXSER_ERR_IOADDR;
+			"mxser(IO)")) {
+		printk(KERN_ERR "mxser: can't request ports I/O region: "
+				"0x%.8lx-0x%.8lx\n",
+				brd->ports[0].ioaddr, brd->ports[0].ioaddr +
+				8 * brd->info->nports - 1);
+		return -EIO;
+	}
 	if (!request_region(brd->vector, 1, "mxser(vector)")) {
 		release_region(brd->ports[0].ioaddr, 8 * brd->info->nports);
-		return MXSER_ERR_VECTOR;
+		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 brd->info->nports;
+
+err_irqconflict:
+	printk(KERN_ERR "mxser: invalid interrupt number\n");
+	return -EIO;
 }
 
 static int __devinit mxser_probe(struct pci_dev *pdev,
@@ -2657,20 +2636,20 @@ static int __devinit mxser_probe(struct pci_dev *pdev,
 			break;
 
 	if (i >= MXSER_BOARDS) {
-		printk(KERN_ERR "Too many Smartio/Industio family boards found "
-			"(maximum %d), board not configured\n", MXSER_BOARDS);
+		dev_err(&pdev->dev, "too many boards found (maximum %d), board "
+				"not configured\n", MXSER_BOARDS);
 		goto err;
 	}
 
 	brd = &mxser_boards[i];
 	brd->idx = i * MXSER_PORTS_PER_BOARD;
-	printk(KERN_INFO "Found MOXA %s board (BusNo=%d, DevNo=%d)\n",
+	dev_info(&pdev->dev, "found MOXA %s board (BusNo=%d, DevNo=%d)\n",
 		mxser_cards[ent->driver_data].name,
 		pdev->bus->number, PCI_SLOT(pdev->devfn));
 
 	retval = pci_enable_device(pdev);
 	if (retval) {
-		printk(KERN_ERR "Moxa SmartI/O PCI enable fail !\n");
+		dev_err(&pdev->dev, "PCI enable failed\n");
 		goto err;
 	}
 
@@ -2772,11 +2751,8 @@ static struct pci_driver mxser_driver = {
 static int __init mxser_module_init(void)
 {
 	struct mxser_board *brd;
-	unsigned long cap;
-	unsigned int i, m, isaloop;
-	int retval, b;
-
-	pr_debug("Loading module mxser ...\n");
+	unsigned int b, i, m;
+	int retval;
 
 	mxvar_sdriver = alloc_tty_driver(MXSER_PORTS + 1);
 	if (!mxvar_sdriver)
@@ -2806,74 +2782,43 @@ static int __init mxser_module_init(void)
 		goto err_put;
 	}
 
-	mxvar_diagflag = 0;
-
-	m = 0;
 	/* Start finding ISA boards here */
-	for (isaloop = 0; isaloop < 2; isaloop++)
-		for (b = 0; b < MXSER_BOARDS && m < MXSER_BOARDS; b++) {
-			if (!isaloop)
-				cap = mxserBoardCAP[b]; /* predefined */
-			else
-				cap = ioaddr[b]; /* module param */
-
-			if (!cap)
-				continue;
+	for (m = 0, b = 0; b < MXSER_BOARDS; b++) {
+		if (!ioaddr[b])
+			continue;
+
+		brd = &mxser_boards[m];
+		retval = mxser_get_ISA_conf(!ioaddr[b], brd);
+		if (retval <= 0) {
+			brd->info = NULL;
+			continue;
+		}
 
-			brd = &mxser_boards[m];
-			retval = mxser_get_ISA_conf(cap, brd);
-
-			if (retval != 0)
-				printk(KERN_INFO "Found MOXA %s board "
-					"(CAP=0x%x)\n",
-					brd->info->name, ioaddr[b]);
-
-			if (retval <= 0) {
-				if (retval == MXSER_ERR_IRQ)
-					printk(KERN_ERR "Invalid interrupt "
-						"number, board not "
-						"configured\n");
-				else if (retval == MXSER_ERR_IRQ_CONFLIT)
-					printk(KERN_ERR "Invalid interrupt "
-						"number, board not "
-						"configured\n");
-				else if (retval == MXSER_ERR_VECTOR)
-					printk(KERN_ERR "Invalid interrupt "
-						"vector, board not "
-						"configured\n");
-				else if (retval == MXSER_ERR_IOADDR)
-					printk(KERN_ERR "Invalid I/O address, "
-						"board not configured\n");
-
-				brd->info = NULL;
-				continue;
-			}
+		printk(KERN_INFO "mxser: found MOXA %s board (CAP=0x%lx)\n",
+				brd->info->name, ioaddr[b]);
 
-			/* mxser_initbrd will hook ISR. */
-			if (mxser_initbrd(brd, NULL) < 0) {
-				brd->info = NULL;
-				continue;
-			}
+		/* mxser_initbrd will hook ISR. */
+		if (mxser_initbrd(brd, NULL) < 0) {
+			brd->info = NULL;
+			continue;
+		}
 
-			brd->idx = m * MXSER_PORTS_PER_BOARD;
-			for (i = 0; i < brd->info->nports; i++)
-				tty_register_device(mxvar_sdriver, brd->idx + i,
-						NULL);
+		brd->idx = m * MXSER_PORTS_PER_BOARD;
+		for (i = 0; i < brd->info->nports; i++)
+			tty_register_device(mxvar_sdriver, brd->idx + i, NULL);
 
-			m++;
-		}
+		m++;
+	}
 
 	retval = pci_register_driver(&mxser_driver);
 	if (retval) {
-		printk(KERN_ERR "Can't register pci driver\n");
+		printk(KERN_ERR "mxser: can't register pci driver\n");
 		if (!m) {
 			retval = -ENODEV;
 			goto err_unr;
 		} /* else: we have some ISA cards under control */
 	}
 
-	pr_debug("Done.\n");
-
 	return 0;
 err_unr:
 	tty_unregister_driver(mxvar_sdriver);
@@ -2886,8 +2831,6 @@ static void __exit mxser_module_exit(void)
 {
 	unsigned int i, j;
 
-	pr_debug("Unloading module mxser ...\n");
-
 	pci_unregister_driver(&mxser_driver);
 
 	for (i = 0; i < MXSER_BOARDS; i++) /* ISA remains */
@@ -2901,8 +2844,6 @@ static void __exit mxser_module_exit(void)
 	for (i = 0; i < MXSER_BOARDS; i++)
 		if (mxser_boards[i].info != NULL)
 			mxser_release_res(&mxser_boards[i], NULL, 1);
-
-	pr_debug("Done.\n");
 }
 
 module_init(mxser_module_init);
diff --git a/drivers/char/nwflash.c b/drivers/char/nwflash.c
index ba012c2bdf7..f9f72a21129 100644
--- a/drivers/char/nwflash.c
+++ b/drivers/char/nwflash.c
@@ -122,35 +122,20 @@ static int flash_ioctl(struct inode *inodep, struct file *filep, unsigned int cm
 static ssize_t flash_read(struct file *file, char __user *buf, size_t size,
 			  loff_t *ppos)
 {
-	unsigned long p = *ppos;
-	unsigned int count = size;
-	int ret = 0;
+	ssize_t ret;
 
 	if (flashdebug)
 		printk(KERN_DEBUG "flash_read: flash_read: offset=0x%lX, "
 		       "buffer=%p, count=0x%X.\n", p, buf, count);
+	/*
+	 * We now lock against reads and writes. --rmk
+	 */
+	if (mutex_lock_interruptible(&nwflash_mutex))
+		return -ERESTARTSYS;
 
-	if (count)
-		ret = -ENXIO;
-
-	if (p < gbFlashSize) {
-		if (count > gbFlashSize - p)
-			count = gbFlashSize - p;
+	ret = simple_read_from_buffer(buf, size, ppos, FLASH_BASE, gbFlashSize);
+	mutex_unlock(&nwflash_mutex);
 
-		/*
-		 * We now lock against reads and writes. --rmk
-		 */
-		if (mutex_lock_interruptible(&nwflash_mutex))
-			return -ERESTARTSYS;
-
-		ret = copy_to_user(buf, (void *)(FLASH_BASE + p), count);
-		if (ret == 0) {
-			ret = count;
-			*ppos += count;
-		} else
-			ret = -EFAULT;
-		mutex_unlock(&nwflash_mutex);
-	}
 	return ret;
 }
 
diff --git a/drivers/char/ppdev.c b/drivers/char/ppdev.c
index 7af7a7e6b9c..bee39fdfba7 100644
--- a/drivers/char/ppdev.c
+++ b/drivers/char/ppdev.c
@@ -67,7 +67,7 @@
 #include <linux/major.h>
 #include <linux/ppdev.h>
 #include <linux/smp_lock.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #define PP_VERSION "ppdev: user-space parallel port driver"
 #define CHRDEV "ppdev"
@@ -328,10 +328,9 @@ static enum ieee1284_phase init_phase (int mode)
 	return IEEE1284_PH_FWD_IDLE;
 }
 
-static int pp_ioctl(struct inode *inode, struct file *file,
-		    unsigned int cmd, unsigned long arg)
+static int pp_do_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
-	unsigned int minor = iminor(inode);
+	unsigned int minor = iminor(file->f_path.dentry->d_inode);
 	struct pp_struct *pp = file->private_data;
 	struct parport * port;
 	void __user *argp = (void __user *)arg;
@@ -634,6 +633,15 @@ static int pp_ioctl(struct inode *inode, struct file *file,
 	return 0;
 }
 
+static long pp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	long ret;
+	lock_kernel();
+	ret = pp_do_ioctl(file, cmd, arg);
+	unlock_kernel();
+	return ret;
+}
+
 static int pp_open (struct inode * inode, struct file * file)
 {
 	unsigned int minor = iminor(inode);
@@ -745,7 +753,7 @@ static const struct file_operations pp_fops = {
 	.read		= pp_read,
 	.write		= pp_write,
 	.poll		= pp_poll,
-	.ioctl		= pp_ioctl,
+	.unlocked_ioctl	= pp_ioctl,
 	.open		= pp_open,
 	.release	= pp_release,
 };
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 0cf98bd4f2d..e0d0e371909 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -236,6 +236,7 @@
 #include <linux/fs.h>
 #include <linux/genhd.h>
 #include <linux/interrupt.h>
+#include <linux/mm.h>
 #include <linux/spinlock.h>
 #include <linux/percpu.h>
 #include <linux/cryptohash.h>
diff --git a/drivers/char/rio/rio_linux.c b/drivers/char/rio/rio_linux.c
index 0cdfee15291..a8f68a3f14d 100644
--- a/drivers/char/rio/rio_linux.c
+++ b/drivers/char/rio/rio_linux.c
@@ -179,7 +179,7 @@ static int rio_set_real_termios(void *ptr);
 static void rio_hungup(void *ptr);
 static void rio_close(void *ptr);
 static int rio_chars_in_buffer(void *ptr);
-static int rio_fw_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg);
+static long rio_fw_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
 static int rio_init_drivers(void);
 
 static void my_hd(void *addr, int len);
@@ -240,7 +240,7 @@ static struct real_driver rio_real_driver = {
 
 static const struct file_operations rio_fw_fops = {
 	.owner = THIS_MODULE,
-	.ioctl = rio_fw_ioctl,
+	.unlocked_ioctl = rio_fw_ioctl,
 };
 
 static struct miscdevice rio_fw_device = {
@@ -560,13 +560,15 @@ static void rio_close(void *ptr)
 
 
 
-static int rio_fw_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
+static long rio_fw_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
 	int rc = 0;
 	func_enter();
 
 	/* The "dev" argument isn't used. */
+	lock_kernel();
 	rc = riocontrol(p, 0, cmd, arg, capable(CAP_SYS_ADMIN));
+	unlock_kernel();
 
 	func_exit();
 	return rc;
diff --git a/drivers/char/rtc.c b/drivers/char/rtc.c
index fa92a8af5a5..d9799e2bcfb 100644
--- a/drivers/char/rtc.c
+++ b/drivers/char/rtc.c
@@ -78,9 +78,10 @@
 #include <linux/wait.h>
 #include <linux/bcd.h>
 #include <linux/delay.h>
+#include <linux/smp_lock.h>
+#include <linux/uaccess.h>
 
 #include <asm/current.h>
-#include <asm/uaccess.h>
 #include <asm/system.h>
 
 #ifdef CONFIG_X86
@@ -120,8 +121,6 @@ static irqreturn_t hpet_rtc_interrupt(int irq, void *dev_id)
 	return 0;
 }
 #endif
-#else
-extern irqreturn_t hpet_rtc_interrupt(int irq, void *dev_id);
 #endif
 
 /*
@@ -144,8 +143,8 @@ static DEFINE_TIMER(rtc_irq_timer, rtc_dropped_irq, 0, 0);
 static ssize_t rtc_read(struct file *file, char __user *buf,
 			size_t count, loff_t *ppos);
 
-static int rtc_ioctl(struct inode *inode, struct file *file,
-		     unsigned int cmd, unsigned long arg);
+static long rtc_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
+static void rtc_get_rtc_time(struct rtc_time *rtc_tm);
 
 #ifdef RTC_IRQ
 static unsigned int rtc_poll(struct file *file, poll_table *wait);
@@ -237,7 +236,7 @@ static inline unsigned char rtc_is_updating(void)
  *	(See ./arch/XXXX/kernel/time.c for the set_rtc_mmss() function.)
  */
 
-irqreturn_t rtc_interrupt(int irq, void *dev_id)
+static irqreturn_t rtc_interrupt(int irq, void *dev_id)
 {
 	/*
 	 *	Can be an alarm interrupt, update complete interrupt,
@@ -719,10 +718,13 @@ static int rtc_do_ioctl(unsigned int cmd, unsigned long arg, int kernel)
 			    &wtime, sizeof wtime) ? -EFAULT : 0;
 }
 
-static int rtc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
-		     unsigned long arg)
+static long rtc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
-	return rtc_do_ioctl(cmd, arg, 0);
+	long ret;
+	lock_kernel();
+	ret = rtc_do_ioctl(cmd, arg, 0);
+	unlock_kernel();
+	return ret;
 }
 
 /*
@@ -915,7 +917,7 @@ static const struct file_operations rtc_fops = {
 #ifdef RTC_IRQ
 	.poll		= rtc_poll,
 #endif
-	.ioctl		= rtc_ioctl,
+	.unlocked_ioctl	= rtc_ioctl,
 	.open		= rtc_open,
 	.release	= rtc_release,
 	.fasync		= rtc_fasync,
@@ -1302,7 +1304,7 @@ static int rtc_proc_open(struct inode *inode, struct file *file)
 }
 #endif
 
-void rtc_get_rtc_time(struct rtc_time *rtc_tm)
+static void rtc_get_rtc_time(struct rtc_time *rtc_tm)
 {
 	unsigned long uip_watchdog = jiffies, flags;
 	unsigned char ctrl;
diff --git a/drivers/char/ser_a2232.c b/drivers/char/ser_a2232.c
index 4ba3aec9e1c..7b0c35207d9 100644
--- a/drivers/char/ser_a2232.c
+++ b/drivers/char/ser_a2232.c
@@ -192,7 +192,7 @@ static inline void a2232_receive_char(struct a2232_port *port, int ch, int err)
 	Maybe one could implement a more efficient version by not only
 	transferring one character at a time.
 */
-	struct tty_struct *tty = port->gs.tty;
+	struct tty_struct *tty = port->gs.port.tty;
 
 #if 0
 	switch(err) {
@@ -226,7 +226,7 @@ static void a2232_disable_tx_interrupts(void *ptr)
 
 	/* Does this here really have to be? */
 	local_irq_save(flags);
-	port->gs.flags &= ~GS_TX_INTEN;
+	port->gs.port.flags &= ~GS_TX_INTEN;
 	local_irq_restore(flags);
 }
 
@@ -242,7 +242,7 @@ static void a2232_enable_tx_interrupts(void *ptr)
 
 	/* Does this here really have to be? */
 	local_irq_save(flags);
-	port->gs.flags |= GS_TX_INTEN;
+	port->gs.port.flags |= GS_TX_INTEN;
 	local_irq_restore(flags);
 }
 
@@ -276,9 +276,9 @@ static void a2232_shutdown_port(void *ptr)
 
 	local_irq_save(flags);
 
-	port->gs.flags &= ~GS_ACTIVE;
+	port->gs.port.flags &= ~GS_ACTIVE;
 	
-	if (port->gs.tty && port->gs.tty->termios->c_cflag & HUPCL) {
+	if (port->gs.port.tty && port->gs.port.tty->termios->c_cflag & HUPCL) {
 		/* Set DTR and RTS to Low, flush output.
 		   The NetBSD driver "msc.c" does it this way. */
 		stat->Command = (	(stat->Command & ~A2232CMD_CMask) | 
@@ -309,7 +309,7 @@ static int  a2232_set_real_termios(void *ptr)
 	volatile struct a2232status *status;
 	volatile struct a2232memory *mem;
 
-	if (!port->gs.tty || !port->gs.tty->termios) return 0;
+	if (!port->gs.port.tty || !port->gs.port.tty->termios) return 0;
 
 	status = a2232stat(port->which_a2232, port->which_port_on_a2232);
 	mem = a2232mem(port->which_a2232);
@@ -345,7 +345,7 @@ static int  a2232_set_real_termios(void *ptr)
 	}
 	a2232_param |= rate;
 
-	cflag  = port->gs.tty->termios->c_cflag;
+	cflag  = port->gs.port.tty->termios->c_cflag;
 
 	// get character size
 	chsize = cflag & CSIZE;
@@ -382,7 +382,7 @@ static int  a2232_set_real_termios(void *ptr)
 		the conventional way of inserting START/STOP characters
 		by hand in throttle()/unthrottle().
 	*/
-	softflow = !!( port->gs.tty->termios->c_iflag & IXOFF );
+	softflow = !!( port->gs.port.tty->termios->c_iflag & IXOFF );
 
 	// get Parity (Enabled/Disabled? If Enabled, Odd or Even?)
 	parity = cflag & (PARENB | PARODD);
@@ -400,9 +400,9 @@ static int  a2232_set_real_termios(void *ptr)
 	/*	Hmm. Maybe an own a2232_port structure
 		member would be cleaner?	*/
 	if (cflag & CLOCAL)
-		port->gs.flags &= ~ASYNC_CHECK_CD;
+		port->gs.port.flags &= ~ASYNC_CHECK_CD;
 	else
-		port->gs.flags |= ASYNC_CHECK_CD;
+		port->gs.port.flags |= ASYNC_CHECK_CD;
 
 
 	/* Now we have all parameters and can go to set them: */
@@ -482,18 +482,18 @@ static int  a2232_open(struct tty_struct * tty, struct file * filp)
 	port = &a2232_ports[line];
 	
 	tty->driver_data = port;
-	port->gs.tty = tty;
-	port->gs.count++;
+	port->gs.port.tty = tty;
+	port->gs.port.count++;
 	retval = gs_init_port(&port->gs);
 	if (retval) {
-		port->gs.count--;
+		port->gs.port.count--;
 		return retval;
 	}
-	port->gs.flags |= GS_ACTIVE;
+	port->gs.port.flags |= GS_ACTIVE;
 	retval = gs_block_til_ready(port, filp);
 
 	if (retval) {
-		port->gs.count--;
+		port->gs.port.count--;
 		return retval;
 	}
 
@@ -522,7 +522,7 @@ int ch, err, n, p;
 		for (p = 0; p < NUMLINES; p++){	/* for every port on this board */
 			err = 0;
 			port = &a2232_ports[n*NUMLINES+p];
-			if ( port->gs.flags & GS_ACTIVE ){ /* if the port is used */
+			if ( port->gs.port.flags & GS_ACTIVE ){ /* if the port is used */
 
 				status = a2232stat(n,p);
 
@@ -577,8 +577,8 @@ int ch, err, n, p;
 				obuf = mem->OutBuf[p];
 				bufpos = status->OutHead;
 				while ( (port->gs.xmit_cnt > 0)		&&
-					(!port->gs.tty->stopped)	&&
-					(!port->gs.tty->hw_stopped) ){	/* While there are chars to transmit */
+					(!port->gs.port.tty->stopped)	&&
+					(!port->gs.port.tty->hw_stopped) ){	/* While there are chars to transmit */
 					if (((bufpos+1) & A2232_IOBUFLENMASK) != status->OutTail) { /* If the A2232 buffer is not full */
 						ch = port->gs.xmit_buf[port->gs.xmit_tail];					/* get the next char to transmit */
 						port->gs.xmit_tail = (port->gs.xmit_tail+1) & (SERIAL_XMIT_SIZE-1); /* modulo-addition for the gs.xmit_buf ring-buffer */
@@ -592,8 +592,8 @@ int ch, err, n, p;
 				status->OutHead = bufpos;
 					
 				/* WakeUp if output buffer runs low */
-				if ((port->gs.xmit_cnt <= port->gs.wakeup_chars) && port->gs.tty) {
-					tty_wakeup(port->gs.tty);
+				if ((port->gs.xmit_cnt <= port->gs.wakeup_chars) && port->gs.port.tty) {
+					tty_wakeup(port->gs.port.tty);
 				}
 			} // if the port is used
 		} // for every port on the board
@@ -613,16 +613,16 @@ int ch, err, n, p;
 						struct a2232_port *port = &a2232_ports[n*7+p];
 						port->cd_status = !(ncd & 1); /* ncd&1 <=> CD is now off */
 
-						if (!(port->gs.flags & ASYNC_CHECK_CD))
+						if (!(port->gs.port.flags & ASYNC_CHECK_CD))
 							;	/* Don't report DCD changes */
 						else if (port->cd_status) { // if DCD on: DCD went UP!
 							
 							/* Are we blocking in open?*/
-							wake_up_interruptible(&port->gs.open_wait);
+							wake_up_interruptible(&port->gs.port.open_wait);
 						}
 						else { // if DCD off: DCD went DOWN!
-							if (port->gs.tty)
-								tty_hangup (port->gs.tty);
+							if (port->gs.port.tty)
+								tty_hangup (port->gs.port.tty);
 						}
 						
 					} // if CD changed for this port
@@ -655,8 +655,8 @@ static void a2232_init_portstructs(void)
 #ifdef NEW_WRITE_LOCKING
 		mutex_init(&(port->gs.port_write_mutex));
 #endif
-		init_waitqueue_head(&port->gs.open_wait);
-		init_waitqueue_head(&port->gs.close_wait);
+		init_waitqueue_head(&port->gs.port.open_wait);
+		init_waitqueue_head(&port->gs.port.close_wait);
 	}
 }
 
diff --git a/drivers/char/stallion.c b/drivers/char/stallion.c
index b976248e107..19db1eb87c2 100644
--- a/drivers/char/stallion.c
+++ b/drivers/char/stallion.c
@@ -1256,7 +1256,6 @@ static int stl_tiocmset(struct tty_struct *tty, struct file *file,
 static int stl_ioctl(struct tty_struct *tty, struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct stlport	*portp;
-	unsigned int	ival;
 	int		rc;
 	void __user *argp = (void __user *)arg;
 
diff --git a/drivers/char/sx.c b/drivers/char/sx.c
index 2162439bbe4..c385206f9db 100644
--- a/drivers/char/sx.c
+++ b/drivers/char/sx.c
@@ -286,8 +286,8 @@ static void sx_close(void *ptr);
 static int sx_chars_in_buffer(void *ptr);
 static int sx_init_board(struct sx_board *board);
 static int sx_init_portstructs(int nboards, int nports);
-static int sx_fw_ioctl(struct inode *inode, struct file *filp,
-		unsigned int cmd, unsigned long arg);
+static long sx_fw_ioctl(struct file *filp, unsigned int cmd,
+						unsigned long arg);
 static int sx_init_drivers(void);
 
 static struct tty_driver *sx_driver;
@@ -396,7 +396,7 @@ static struct real_driver sx_real_driver = {
 
 static const struct file_operations sx_fw_fops = {
 	.owner = THIS_MODULE,
-	.ioctl = sx_fw_ioctl,
+	.unlocked_ioctl = sx_fw_ioctl,
 };
 
 static struct miscdevice sx_fw_device = {
@@ -1686,10 +1686,10 @@ static int do_memtest_w(struct sx_board *board, int min, int max)
 }
 #endif
 
-static int sx_fw_ioctl(struct inode *inode, struct file *filp,
-		unsigned int cmd, unsigned long arg)
+static long sx_fw_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
 {
-	int rc = 0;
+	long rc = 0;
 	int __user *descr = (int __user *)arg;
 	int i;
 	static struct sx_board *board = NULL;
@@ -1699,13 +1699,10 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 
 	func_enter();
 
-#if 0
-	/* Removed superuser check: Sysops can use the permissions on the device
-	   file to restrict access. Recommendation: Root only. (root.root 600) */
-	if (!capable(CAP_SYS_ADMIN)) {
+	if (!capable(CAP_SYS_RAWIO))
 		return -EPERM;
-	}
-#endif
+
+	lock_kernel();
 
 	sx_dprintk(SX_DEBUG_FIRMWARE, "IOCTL %x: %lx\n", cmd, arg);
 
@@ -1720,19 +1717,23 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 		for (i = 0; i < SX_NBOARDS; i++)
 			sx_dprintk(SX_DEBUG_FIRMWARE, "<%x> ", boards[i].flags);
 		sx_dprintk(SX_DEBUG_FIRMWARE, "\n");
+		unlock_kernel();
 		return -EIO;
 	}
 
 	switch (cmd) {
 	case SXIO_SET_BOARD:
 		sx_dprintk(SX_DEBUG_FIRMWARE, "set board to %ld\n", arg);
+		rc = -EIO;
 		if (arg >= SX_NBOARDS)
-			return -EIO;
+			break;
 		sx_dprintk(SX_DEBUG_FIRMWARE, "not out of range\n");
 		if (!(boards[arg].flags & SX_BOARD_PRESENT))
-			return -EIO;
+			break;
 		sx_dprintk(SX_DEBUG_FIRMWARE, ".. and present!\n");
 		board = &boards[arg];
+		rc = 0;
+		/* FIXME: And this does ... nothing?? */
 		break;
 	case SXIO_GET_TYPE:
 		rc = -ENOENT;	/* If we manage to miss one, return error. */
@@ -1746,7 +1747,7 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 			rc = SX_TYPE_SI;
 		if (IS_EISA_BOARD(board))
 			rc = SX_TYPE_SI;
-		sx_dprintk(SX_DEBUG_FIRMWARE, "returning type= %d\n", rc);
+		sx_dprintk(SX_DEBUG_FIRMWARE, "returning type= %ld\n", rc);
 		break;
 	case SXIO_DO_RAMTEST:
 		if (sx_initialized)	/* Already initialized: better not ramtest the board.  */
@@ -1760,19 +1761,26 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 			rc = do_memtest(board, 0, 0x7ff8);
 			/* if (!rc) rc = do_memtest_w (board, 0, 0x7ff8); */
 		}
-		sx_dprintk(SX_DEBUG_FIRMWARE, "returning memtest result= %d\n",
-			   rc);
+		sx_dprintk(SX_DEBUG_FIRMWARE,
+				"returning memtest result= %ld\n", rc);
 		break;
 	case SXIO_DOWNLOAD:
-		if (sx_initialized)	/* Already initialized */
-			return -EEXIST;
-		if (!sx_reset(board))
-			return -EIO;
+		if (sx_initialized) {/* Already initialized */
+			rc = -EEXIST;
+			break;
+		}
+		if (!sx_reset(board)) {
+			rc = -EIO;
+			break;
+		}
 		sx_dprintk(SX_DEBUG_INIT, "reset the board...\n");
 
 		tmp = kmalloc(SX_CHUNK_SIZE, GFP_USER);
-		if (!tmp)
-			return -ENOMEM;
+		if (!tmp) {
+			rc = -ENOMEM;
+			break;
+		}
+		/* FIXME: check returns */
 		get_user(nbytes, descr++);
 		get_user(offset, descr++);
 		get_user(data, descr++);
@@ -1782,7 +1790,8 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 						(i + SX_CHUNK_SIZE > nbytes) ?
 						nbytes - i : SX_CHUNK_SIZE)) {
 					kfree(tmp);
-					return -EFAULT;
+					rc = -EFAULT;
+					break;
 				}
 				memcpy_toio(board->base2 + offset + i, tmp,
 						(i + SX_CHUNK_SIZE > nbytes) ?
@@ -1798,13 +1807,17 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 		rc = sx_nports;
 		break;
 	case SXIO_INIT:
-		if (sx_initialized)	/* Already initialized */
-			return -EEXIST;
+		if (sx_initialized) {	/* Already initialized */
+			rc = -EEXIST;
+			break;
+		}
 		/* This is not allowed until all boards are initialized... */
 		for (i = 0; i < SX_NBOARDS; i++) {
 			if ((boards[i].flags & SX_BOARD_PRESENT) &&
-				!(boards[i].flags & SX_BOARD_INITIALIZED))
-				return -EIO;
+				!(boards[i].flags & SX_BOARD_INITIALIZED)) {
+				rc = -EIO;
+				break;
+			}
 		}
 		for (i = 0; i < SX_NBOARDS; i++)
 			if (!(boards[i].flags & SX_BOARD_PRESENT))
@@ -1832,10 +1845,10 @@ static int sx_fw_ioctl(struct inode *inode, struct file *filp,
 		rc = sx_nports;
 		break;
 	default:
-		printk(KERN_WARNING "Unknown ioctl on firmware device (%x).\n",
-				cmd);
+		rc = -ENOTTY;
 		break;
 	}
+	unlock_kernel();
 	func_exit();
 	return rc;
 }
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index e1fc193d939..ae766d86845 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -580,91 +580,133 @@ void tpm_continue_selftest(struct tpm_chip *chip)
 }
 EXPORT_SYMBOL_GPL(tpm_continue_selftest);
 
+#define  TPM_INTERNAL_RESULT_SIZE 200
+
 ssize_t tpm_show_enabled(struct device * dev, struct device_attribute * attr,
 			char *buf)
 {
-	u8 data[max_t(int, ARRAY_SIZE(tpm_cap), 35)];
+	u8 *data;
 	ssize_t rc;
 
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_FLAG;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_FLAG_PERM;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
-			"attemtping to determine the permanent state");
-	if (rc)
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
+			"attemtping to determine the permanent enabled state");
+	if (rc) {
+		kfree(data);
 		return 0;
-	return sprintf(buf, "%d\n", !data[TPM_GET_CAP_PERM_DISABLE_IDX]);
+	}
+
+	rc = sprintf(buf, "%d\n", !data[TPM_GET_CAP_PERM_DISABLE_IDX]);
+
+	kfree(data);
+	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_show_enabled);
 
 ssize_t tpm_show_active(struct device * dev, struct device_attribute * attr,
 			char *buf)
 {
-	u8 data[max_t(int, ARRAY_SIZE(tpm_cap), 35)];
+	u8 *data;
 	ssize_t rc;
 
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_FLAG;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_FLAG_PERM;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
-			"attemtping to determine the permanent state");
-	if (rc)
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
+			"attemtping to determine the permanent active state");
+	if (rc) {
+		kfree(data);
 		return 0;
-	return sprintf(buf, "%d\n", !data[TPM_GET_CAP_PERM_INACTIVE_IDX]);
+	}
+
+	rc = sprintf(buf, "%d\n", !data[TPM_GET_CAP_PERM_INACTIVE_IDX]);
+
+	kfree(data);
+	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_show_active);
 
 ssize_t tpm_show_owned(struct device * dev, struct device_attribute * attr,
 			char *buf)
 {
-	u8 data[sizeof(tpm_cap)];
+	u8 *data;
 	ssize_t rc;
 
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_PROP;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_PROP_OWNER;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 			"attempting to determine the owner state");
-	if (rc)
+	if (rc) {
+		kfree(data);
 		return 0;
-	return sprintf(buf, "%d\n", data[TPM_GET_CAP_RET_BOOL_1_IDX]);
+	}
+
+	rc = sprintf(buf, "%d\n", data[TPM_GET_CAP_RET_BOOL_1_IDX]);
+
+	kfree(data);
+	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_show_owned);
 
 ssize_t tpm_show_temp_deactivated(struct device * dev,
 				struct device_attribute * attr, char *buf)
 {
-	u8 data[sizeof(tpm_cap)];
+	u8 *data;
 	ssize_t rc;
 
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_FLAG;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_FLAG_VOL;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 			"attempting to determine the temporary state");
-	if (rc)
+	if (rc) {
+		kfree(data);
 		return 0;
-	return sprintf(buf, "%d\n", data[TPM_GET_CAP_TEMP_INACTIVE_IDX]);
+	}
+
+	rc = sprintf(buf, "%d\n", data[TPM_GET_CAP_TEMP_INACTIVE_IDX]);
+
+	kfree(data);
+	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_show_temp_deactivated);
 
@@ -678,7 +720,7 @@ static const u8 pcrread[] = {
 ssize_t tpm_show_pcrs(struct device *dev, struct device_attribute *attr,
 		      char *buf)
 {
-	u8 data[max_t(int, max(ARRAY_SIZE(tpm_cap), ARRAY_SIZE(pcrread)), 30)];
+	u8 *data;
 	ssize_t rc;
 	int i, j, num_pcrs;
 	__be32 index;
@@ -688,21 +730,27 @@ ssize_t tpm_show_pcrs(struct device *dev, struct device_attribute *attr,
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_PROP;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_PROP_PCR;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 			"attempting to determine the number of PCRS");
-	if (rc)
+	if (rc) {
+		kfree(data);
 		return 0;
+	}
 
 	num_pcrs = be32_to_cpu(*((__be32 *) (data + 14)));
 	for (i = 0; i < num_pcrs; i++) {
 		memcpy(data, pcrread, sizeof(pcrread));
 		index = cpu_to_be32(i);
 		memcpy(data + 10, &index, 4);
-		rc = transmit_cmd(chip, data, sizeof(data),
+		rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 				"attempting to read a PCR");
 		if (rc)
 			goto out;
@@ -712,6 +760,7 @@ ssize_t tpm_show_pcrs(struct device *dev, struct device_attribute *attr,
 		str += sprintf(str, "\n");
 	}
 out:
+	kfree(data);
 	return str - buf;
 }
 EXPORT_SYMBOL_GPL(tpm_show_pcrs);
@@ -795,7 +844,7 @@ static const u8 cap_version[] = {
 ssize_t tpm_show_caps(struct device *dev, struct device_attribute *attr,
 		      char *buf)
 {
-	u8 data[max_t(int, max(ARRAY_SIZE(tpm_cap), ARRAY_SIZE(cap_version)), 30)];
+	u8 *data;
 	ssize_t rc;
 	char *str = buf;
 
@@ -803,21 +852,27 @@ ssize_t tpm_show_caps(struct device *dev, struct device_attribute *attr,
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_PROP;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_PROP_MANUFACTURER;
 
-	rc = transmit_cmd(chip, data, sizeof(data),
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 			"attempting to determine the manufacturer");
-	if (rc)
+	if (rc) {
+		kfree(data);
 		return 0;
+	}
 
 	str += sprintf(str, "Manufacturer: 0x%x\n",
 		       be32_to_cpu(*((__be32 *) (data + TPM_GET_CAP_RET_UINT32_1_IDX))));
 
 	memcpy(data, cap_version, sizeof(cap_version));
 	data[CAP_VERSION_IDX] = CAP_VERSION_1_1;
-	rc = transmit_cmd(chip, data, sizeof(data),
+	rc = transmit_cmd(chip, data, TPM_INTERNAL_RESULT_SIZE,
 			"attempting to determine the 1.1 version");
 	if (rc)
 		goto out;
@@ -828,6 +883,7 @@ ssize_t tpm_show_caps(struct device *dev, struct device_attribute *attr,
 		       (int) data[17]);
 
 out:
+	kfree(data);
 	return str - buf;
 }
 EXPORT_SYMBOL_GPL(tpm_show_caps);
@@ -835,7 +891,7 @@ EXPORT_SYMBOL_GPL(tpm_show_caps);
 ssize_t tpm_show_caps_1_2(struct device * dev,
 			  struct device_attribute * attr, char *buf)
 {
-	u8 data[max_t(int, max(ARRAY_SIZE(tpm_cap), ARRAY_SIZE(cap_version)), 30)];
+	u8 *data;
 	ssize_t len;
 	char *str = buf;
 
@@ -843,15 +899,20 @@ ssize_t tpm_show_caps_1_2(struct device * dev,
 	if (chip == NULL)
 		return -ENODEV;
 
+	data = kzalloc(TPM_INTERNAL_RESULT_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	memcpy(data, tpm_cap, sizeof(tpm_cap));
 	data[TPM_CAP_IDX] = TPM_CAP_PROP;
 	data[TPM_CAP_SUBCAP_IDX] = TPM_CAP_PROP_MANUFACTURER;
 
-	if ((len = tpm_transmit(chip, data, sizeof(data))) <=
-	    TPM_ERROR_SIZE) {
+	len = tpm_transmit(chip, data, TPM_INTERNAL_RESULT_SIZE);
+	if (len <= TPM_ERROR_SIZE) {
 		dev_dbg(chip->dev, "A TPM error (%d) occurred "
 			"attempting to determine the manufacturer\n",
 			be32_to_cpu(*((__be32 *) (data + TPM_RET_CODE_IDX))));
+		kfree(data);
 		return 0;
 	}
 
@@ -861,8 +922,8 @@ ssize_t tpm_show_caps_1_2(struct device * dev,
 	memcpy(data, cap_version, sizeof(cap_version));
 	data[CAP_VERSION_IDX] = CAP_VERSION_1_2;
 
-	if ((len = tpm_transmit(chip, data, sizeof(data))) <=
-	    TPM_ERROR_SIZE) {
+	len = tpm_transmit(chip, data, TPM_INTERNAL_RESULT_SIZE);
+	if (len <= TPM_ERROR_SIZE) {
 		dev_err(chip->dev, "A TPM error (%d) occurred "
 			"attempting to determine the 1.2 version\n",
 			be32_to_cpu(*((__be32 *) (data + TPM_RET_CODE_IDX))));
@@ -874,6 +935,7 @@ ssize_t tpm_show_caps_1_2(struct device * dev,
 		       (int) data[19]);
 
 out:
+	kfree(data);
 	return str - buf;
 }
 EXPORT_SYMBOL_GPL(tpm_show_caps_1_2);
@@ -966,7 +1028,7 @@ ssize_t tpm_write(struct file *file, const char __user *buf,
 		  size_t size, loff_t *off)
 {
 	struct tpm_chip *chip = file->private_data;
-	int in_size = size, out_size;
+	size_t in_size = size, out_size;
 
 	/* cannot perform a write until the read has cleared
 	   either via tpm_read or a user_read_timer timeout */
@@ -1001,7 +1063,7 @@ ssize_t tpm_read(struct file *file, char __user *buf,
 		 size_t size, loff_t *off)
 {
 	struct tpm_chip *chip = file->private_data;
-	int ret_size;
+	ssize_t ret_size;
 
 	del_singleshot_timer_sync(&chip->user_read_timer);
 	flush_scheduled_work();
diff --git a/drivers/char/tpm/tpm_bios.c b/drivers/char/tpm/tpm_bios.c
index 60a2d2630e3..68f052b42ed 100644
--- a/drivers/char/tpm/tpm_bios.c
+++ b/drivers/char/tpm/tpm_bios.c
@@ -448,7 +448,7 @@ out_free:
 	goto out;
 }
 
-const struct file_operations tpm_ascii_bios_measurements_ops = {
+static const struct file_operations tpm_ascii_bios_measurements_ops = {
 	.open = tpm_ascii_bios_measurements_open,
 	.read = seq_read,
 	.llseek = seq_lseek,
@@ -486,7 +486,7 @@ out_free:
 	goto out;
 }
 
-const struct file_operations tpm_binary_bios_measurements_ops = {
+static const struct file_operations tpm_binary_bios_measurements_ops = {
 	.open = tpm_binary_bios_measurements_open,
 	.read = seq_read,
 	.llseek = seq_lseek,
diff --git a/drivers/char/tpm/tpm_tis.c b/drivers/char/tpm/tpm_tis.c
index c7a977bc03e..ed1879c0dd8 100644
--- a/drivers/char/tpm/tpm_tis.c
+++ b/drivers/char/tpm/tpm_tis.c
@@ -622,6 +622,7 @@ static struct pnp_device_id tpm_pnp_tbl[] __devinitdata = {
 	{"ATM1200", 0},		/* Atmel */
 	{"IFX0102", 0},		/* Infineon */
 	{"BCM0101", 0},		/* Broadcom */
+	{"BCM0102", 0},		/* Broadcom */
 	{"NSC1200", 0},		/* National */
 	{"ICO0102", 0},		/* Intel */
 	/* Add new here */
diff --git a/drivers/char/tty_io.c b/drivers/char/tty_io.c
index fa48dba5ba5..e1b46bc7e43 100644
--- a/drivers/char/tty_io.c
+++ b/drivers/char/tty_io.c
@@ -1119,19 +1119,6 @@ int tty_hung_up_p(struct file *filp)
 
 EXPORT_SYMBOL(tty_hung_up_p);
 
-/**
- *	is_tty	-	checker whether file is a TTY
- *	@filp:		file handle that may be a tty
- *
- *	Check if the file handle is a tty handle.
- */
-
-int is_tty(struct file *filp)
-{
-	return filp->f_op->read == tty_read
-		|| filp->f_op->read == hung_up_tty_read;
-}
-
 static void session_clear_tty(struct pid *session)
 {
 	struct task_struct *p;
@@ -3593,7 +3580,6 @@ void proc_clear_tty(struct task_struct *p)
 	p->signal->tty = NULL;
 	spin_unlock_irq(&p->sighand->siglock);
 }
-EXPORT_SYMBOL(proc_clear_tty);
 
 /* Called under the sighand lock */
 
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index dc17fe3a88b..d0f4eb6fdb7 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -46,6 +46,9 @@ static char *in, *inbuf;
 /* The operations for our console. */
 static struct hv_ops virtio_cons;
 
+/* The hvc device */
+static struct hvc_struct *hvc;
+
 /*D:310 The put_chars() callback is pretty straightforward.
  *
  * We turn the characters into a scatter-gather list, add it to the output
@@ -134,6 +137,27 @@ int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
 	return hvc_instantiate(0, 0, &virtio_cons);
 }
 
+/*
+ * we support only one console, the hvc struct is a global var
+ * There is no need to do anything
+ */
+static int notifier_add_vio(struct hvc_struct *hp, int data)
+{
+	hp->irq_requested = 1;
+	return 0;
+}
+
+static void notifier_del_vio(struct hvc_struct *hp, int data)
+{
+	hp->irq_requested = 0;
+}
+
+static void hvc_handle_input(struct virtqueue *vq)
+{
+	if (hvc_poll(hvc))
+		hvc_kick();
+}
+
 /*D:370 Once we're further in boot, we get probed like any other virtio device.
  * At this stage we set up the output virtqueue.
  *
@@ -144,7 +168,6 @@ int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
 static int __devinit virtcons_probe(struct virtio_device *dev)
 {
 	int err;
-	struct hvc_struct *hvc;
 
 	vdev = dev;
 
@@ -158,7 +181,7 @@ static int __devinit virtcons_probe(struct virtio_device *dev)
 	/* Find the input queue. */
 	/* FIXME: This is why we want to wean off hvc: we do nothing
 	 * when input comes in. */
-	in_vq = vdev->config->find_vq(vdev, 0, NULL);
+	in_vq = vdev->config->find_vq(vdev, 0, hvc_handle_input);
 	if (IS_ERR(in_vq)) {
 		err = PTR_ERR(in_vq);
 		goto free;
@@ -173,15 +196,18 @@ static int __devinit virtcons_probe(struct virtio_device *dev)
 	/* Start using the new console output. */
 	virtio_cons.get_chars = get_chars;
 	virtio_cons.put_chars = put_chars;
+	virtio_cons.notifier_add = notifier_add_vio;
+	virtio_cons.notifier_del = notifier_del_vio;
 
 	/* The first argument of hvc_alloc() is the virtual console number, so
-	 * we use zero.  The second argument is the interrupt number; we
-	 * currently leave this as zero: it would be better not to use the
-	 * hvc mechanism and fix this (FIXME!).
+	 * we use zero.  The second argument is the parameter for the
+	 * notification mechanism (like irq number). We currently leave this
+	 * as zero, virtqueues have implicit notifications.
 	 *
 	 * The third argument is a "struct hv_ops" containing the put_chars()
-	 * and get_chars() pointers.  The final argument is the output buffer
-	 * size: we can do any size, so we put PAGE_SIZE here. */
+	 * get_chars(), notifier_add() and notifier_del() pointers.
+	 * The final argument is the output buffer size: we can do any size,
+	 * so we put PAGE_SIZE here. */
 	hvc = hvc_alloc(0, 0, &virtio_cons, PAGE_SIZE);
 	if (IS_ERR(hvc)) {
 		err = PTR_ERR(hvc);
diff --git a/drivers/char/vme_scc.c b/drivers/char/vme_scc.c
index 69c5afe97f1..1718b3c481d 100644
--- a/drivers/char/vme_scc.c
+++ b/drivers/char/vme_scc.c
@@ -183,8 +183,8 @@ static void scc_init_portstructs(void)
 #ifdef NEW_WRITE_LOCKING
 		port->gs.port_write_mutex = MUTEX;
 #endif
-		init_waitqueue_head(&port->gs.open_wait);
-		init_waitqueue_head(&port->gs.close_wait);
+		init_waitqueue_head(&port->gs.port.open_wait);
+		init_waitqueue_head(&port->gs.port.close_wait);
 	}
 }
 
@@ -422,7 +422,7 @@ static irqreturn_t scc_rx_int(int irq, void *data)
 {
 	unsigned char	ch;
 	struct scc_port *port = data;
-	struct tty_struct *tty = port->gs.tty;
+	struct tty_struct *tty = port->gs.port.tty;
 	SCC_ACCESS_INIT(port);
 
 	ch = SCCread_NB(RX_DATA_REG);
@@ -453,7 +453,7 @@ static irqreturn_t scc_rx_int(int irq, void *data)
 static irqreturn_t scc_spcond_int(int irq, void *data)
 {
 	struct scc_port *port = data;
-	struct tty_struct *tty = port->gs.tty;
+	struct tty_struct *tty = port->gs.port.tty;
 	unsigned char	stat, ch, err;
 	int		int_pending_mask = port->channel == CHANNEL_A ?
 			                   IPR_A_RX : IPR_B_RX;
@@ -500,7 +500,7 @@ static irqreturn_t scc_tx_int(int irq, void *data)
 	struct scc_port *port = data;
 	SCC_ACCESS_INIT(port);
 
-	if (!port->gs.tty) {
+	if (!port->gs.port.tty) {
 		printk(KERN_WARNING "scc_tx_int with NULL tty!\n");
 		SCCmod (INT_AND_DMA_REG, ~IDR_TX_INT_ENAB, 0);
 		SCCwrite(COMMAND_REG, CR_TX_PENDING_RESET);
@@ -512,8 +512,9 @@ static irqreturn_t scc_tx_int(int irq, void *data)
 			SCCwrite(TX_DATA_REG, port->x_char);
 			port->x_char = 0;
 		}
-		else if ((port->gs.xmit_cnt <= 0) || port->gs.tty->stopped ||
-				port->gs.tty->hw_stopped)
+		else if ((port->gs.xmit_cnt <= 0) ||
+			 port->gs.port.tty->stopped ||
+			 port->gs.port.tty->hw_stopped)
 			break;
 		else {
 			SCCwrite(TX_DATA_REG, port->gs.xmit_buf[port->gs.xmit_tail++]);
@@ -522,15 +523,15 @@ static irqreturn_t scc_tx_int(int irq, void *data)
 				break;
 		}
 	}
-	if ((port->gs.xmit_cnt <= 0) || port->gs.tty->stopped ||
-			port->gs.tty->hw_stopped) {
+	if ((port->gs.xmit_cnt <= 0) || port->gs.port.tty->stopped ||
+	    port->gs.port.tty->hw_stopped) {
 		/* disable tx interrupts */
 		SCCmod (INT_AND_DMA_REG, ~IDR_TX_INT_ENAB, 0);
 		SCCwrite(COMMAND_REG, CR_TX_PENDING_RESET);   /* disable tx_int on next tx underrun? */
-		port->gs.flags &= ~GS_TX_INTEN;
+		port->gs.port.flags &= ~GS_TX_INTEN;
 	}
-	if (port->gs.tty && port->gs.xmit_cnt <= port->gs.wakeup_chars)
-		tty_wakeup(port->gs.tty);
+	if (port->gs.port.tty && port->gs.xmit_cnt <= port->gs.wakeup_chars)
+		tty_wakeup(port->gs.port.tty);
 
 	SCCwrite_NB(COMMAND_REG, CR_HIGHEST_IUS_RESET);
 	return IRQ_HANDLED;
@@ -550,14 +551,14 @@ static irqreturn_t scc_stat_int(int irq, void *data)
 
 	if (changed & SR_DCD) {
 		port->c_dcd = !!(sr & SR_DCD);
-		if (!(port->gs.flags & ASYNC_CHECK_CD))
+		if (!(port->gs.port.flags & ASYNC_CHECK_CD))
 			;	/* Don't report DCD changes */
 		else if (port->c_dcd) {
-			wake_up_interruptible(&port->gs.open_wait);
+			wake_up_interruptible(&port->gs.port.open_wait);
 		}
 		else {
-			if (port->gs.tty)
-				tty_hangup (port->gs.tty);
+			if (port->gs.port.tty)
+				tty_hangup (port->gs.port.tty);
 		}
 	}
 	SCCwrite(COMMAND_REG, CR_EXTSTAT_RESET);
@@ -578,7 +579,7 @@ static void scc_disable_tx_interrupts(void *ptr)
 
 	local_irq_save(flags);
 	SCCmod(INT_AND_DMA_REG, ~IDR_TX_INT_ENAB, 0);
-	port->gs.flags &= ~GS_TX_INTEN;
+	port->gs.port.flags &= ~GS_TX_INTEN;
 	local_irq_restore(flags);
 }
 
@@ -636,8 +637,8 @@ static void scc_shutdown_port(void *ptr)
 {
 	struct scc_port *port = ptr;
 
-	port->gs.flags &= ~ GS_ACTIVE;
-	if (port->gs.tty && port->gs.tty->termios->c_cflag & HUPCL) {
+	port->gs.port.flags &= ~ GS_ACTIVE;
+	if (port->gs.port.tty && port->gs.port.tty->termios->c_cflag & HUPCL) {
 		scc_setsignals (port, 0, 0);
 	}
 }
@@ -652,14 +653,14 @@ static int scc_set_real_termios (void *ptr)
 	struct scc_port *port = ptr;
 	SCC_ACCESS_INIT(port);
 
-	if (!port->gs.tty || !port->gs.tty->termios) return 0;
+	if (!port->gs.port.tty || !port->gs.port.tty->termios) return 0;
 
 	channel = port->channel;
 
 	if (channel == CHANNEL_A)
 		return 0;		/* Settings controlled by boot PROM */
 
-	cflag  = port->gs.tty->termios->c_cflag;
+	cflag  = port->gs.port.tty->termios->c_cflag;
 	baud = port->gs.baud;
 	chsize = (cflag & CSIZE) >> 4;
 
@@ -678,9 +679,9 @@ static int scc_set_real_termios (void *ptr)
 	}
 
 	if (cflag & CLOCAL)
-		port->gs.flags &= ~ASYNC_CHECK_CD;
+		port->gs.port.flags &= ~ASYNC_CHECK_CD;
 	else
-		port->gs.flags |= ASYNC_CHECK_CD;
+		port->gs.port.flags |= ASYNC_CHECK_CD;
 
 #ifdef CONFIG_MVME147_SCC
 	if (MACH_IS_MVME147)
@@ -856,7 +857,7 @@ static int scc_open (struct tty_struct * tty, struct file * filp)
 		{ COMMAND_REG, CR_EXTSTAT_RESET },
 	};
 #endif
-	if (!(port->gs.flags & ASYNC_INITIALIZED)) {
+	if (!(port->gs.port.flags & ASYNC_INITIALIZED)) {
 		local_irq_save(flags);
 #if defined(CONFIG_MVME147_SCC) || defined(CONFIG_MVME162_SCC)
 		if (MACH_IS_MVME147 || MACH_IS_MVME16x) {
@@ -880,18 +881,18 @@ static int scc_open (struct tty_struct * tty, struct file * filp)
 	}
 
 	tty->driver_data = port;
-	port->gs.tty = tty;
-	port->gs.count++;
+	port->gs.port.tty = tty;
+	port->gs.port.count++;
 	retval = gs_init_port(&port->gs);
 	if (retval) {
-		port->gs.count--;
+		port->gs.port.count--;
 		return retval;
 	}
-	port->gs.flags |= GS_ACTIVE;
+	port->gs.port.flags |= GS_ACTIVE;
 	retval = gs_block_til_ready(port, filp);
 
 	if (retval) {
-		port->gs.count--;
+		port->gs.port.count--;
 		return retval;
 	}
 
diff --git a/drivers/char/vt.c b/drivers/char/vt.c
index e32a076d5f1..82a51f38a54 100644
--- a/drivers/char/vt.c
+++ b/drivers/char/vt.c
@@ -261,7 +261,7 @@ static void notify_update(struct vc_data *vc)
 #ifdef VT_BUF_VRAM_ONLY
 #define DO_UPDATE(vc)	0
 #else
-#define DO_UPDATE(vc)	CON_IS_VISIBLE(vc)
+#define DO_UPDATE(vc)	(CON_IS_VISIBLE(vc) && !console_blanked)
 #endif
 
 static inline unsigned short *screenpos(struct vc_data *vc, int offset, int viewed)
@@ -2211,7 +2211,7 @@ rescan_last_byte:
 			c = 0xfffd;
 		    tc = c;
 		} else {	/* no utf or alternate charset mode */
-		    tc = vc->vc_translate[vc->vc_toggle_meta ? (c | 0x80) : c];
+		    tc = vc_translate(vc, c);
 		}
 
 		param.c = tc;
@@ -2749,8 +2749,8 @@ static int con_open(struct tty_struct *tty, struct file *filp)
 				tty->termios->c_iflag |= IUTF8;
 			else
 				tty->termios->c_iflag &= ~IUTF8;
-			release_console_sem();
 			vcs_make_sysfs(tty);
+			release_console_sem();
 			return ret;
 		}
 	}
@@ -2775,8 +2775,8 @@ static void con_close(struct tty_struct *tty, struct file *filp)
 		if (vc)
 			vc->vc_tty = NULL;
 		tty->driver_data = NULL;
-		release_console_sem();
 		vcs_remove_sysfs(tty);
+		release_console_sem();
 		mutex_unlock(&tty_mutex);
 		/*
 		 * tty_mutex is released, but we still hold BKL, so there is
diff --git a/drivers/char/xilinx_hwicap/xilinx_hwicap.c b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
index 51966ccf4ea..8bfee5fb722 100644
--- a/drivers/char/xilinx_hwicap/xilinx_hwicap.c
+++ b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
@@ -87,7 +87,6 @@
 #include <linux/mutex.h>
 #include <linux/smp_lock.h>
 #include <linux/sysctl.h>
-#include <linux/version.h>
 #include <linux/fs.h>
 #include <linux/cdev.h>
 #include <linux/platform_device.h>
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index 6e6c3c4aea6..5a11e3cbcae 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -123,6 +123,13 @@ config EDAC_I5000
 	  Support for error detection and correction the Intel
 	  Greekcreek/Blackford chipsets.
 
+config EDAC_I5100
+	tristate "Intel San Clemente MCH"
+	depends on EDAC_MM_EDAC && X86 && PCI
+	help
+	  Support for error detection and correction the Intel
+	  San Clemente MCH.
+
 config EDAC_MPC85XX
 	tristate "Freescale MPC85xx"
 	depends on EDAC_MM_EDAC && FSL_SOC && MPC85xx
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index 83807731d4a..e5e9104b552 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -19,6 +19,7 @@ endif
 
 obj-$(CONFIG_EDAC_AMD76X)		+= amd76x_edac.o
 obj-$(CONFIG_EDAC_I5000)		+= i5000_edac.o
+obj-$(CONFIG_EDAC_I5100)		+= i5100_edac.o
 obj-$(CONFIG_EDAC_E7XXX)		+= e7xxx_edac.o
 obj-$(CONFIG_EDAC_E752X)		+= e752x_edac.o
 obj-$(CONFIG_EDAC_I82443BXGX)		+= i82443bxgx_edac.o
diff --git a/drivers/edac/e752x_edac.c b/drivers/edac/e752x_edac.c
index c94a0eb492c..facfdb1fa71 100644
--- a/drivers/edac/e752x_edac.c
+++ b/drivers/edac/e752x_edac.c
@@ -28,6 +28,7 @@
 #define E752X_REVISION	" Ver: 2.0.2 " __DATE__
 #define EDAC_MOD_STR	"e752x_edac"
 
+static int report_non_memory_errors;
 static int force_function_unhide;
 static int sysbus_parity = -1;
 
@@ -117,7 +118,7 @@ static struct edac_pci_ctl_info *e752x_pci;
 #define E752X_BUF_FERR		0x70	/* Memory buffer first error reg (8b) */
 #define E752X_BUF_NERR		0x72	/* Memory buffer next error reg (8b) */
 #define E752X_BUF_ERRMASK	0x74	/* Memory buffer error mask reg (8b) */
-#define E752X_BUF_SMICMD	0x7A	/* Memory buffer SMI command reg (8b) */
+#define E752X_BUF_SMICMD	0x7A	/* Memory buffer SMI cmd reg (8b) */
 #define E752X_DRAM_FERR		0x80	/* DRAM first error register (16b) */
 #define E752X_DRAM_NERR		0x82	/* DRAM next error register (16b) */
 #define E752X_DRAM_ERRMASK	0x84	/* DRAM error mask register (8b) */
@@ -127,7 +128,7 @@ static struct edac_pci_ctl_info *e752x_pci;
 					/*     error address register (32b) */
 					/*
 					 * 31    Reserved
-					 * 30:2  CE address (64 byte block 34:6)
+					 * 30:2  CE address (64 byte block 34:6
 					 * 1     Reserved
 					 * 0     HiLoCS
 					 */
@@ -147,11 +148,11 @@ static struct edac_pci_ctl_info *e752x_pci;
 					 * 1     Reserved
 					 * 0     HiLoCS
 					 */
-#define E752X_DRAM_SCRB_ADD	0xA8	/* DRAM first uncorrectable scrub memory */
+#define E752X_DRAM_SCRB_ADD	0xA8	/* DRAM 1st uncorrectable scrub mem */
 					/*     error address register (32b) */
 					/*
 					 * 31    Reserved
-					 * 30:2  CE address (64 byte block 34:6)
+					 * 30:2  CE address (64 byte block 34:6
 					 * 1     Reserved
 					 * 0     HiLoCS
 					 */
@@ -394,9 +395,12 @@ static void do_process_ded_retry(struct mem_ctl_info *mci, u16 error,
 	struct e752x_pvt *pvt = (struct e752x_pvt *)mci->pvt_info;
 
 	error_1b = retry_add;
-	page = error_1b >> (PAGE_SHIFT - 4);	/* convert the addr to 4k page */
-	row = pvt->mc_symmetric ? ((page >> 1) & 3) :	/* chip select are bits 14 & 13 */
+	page = error_1b >> (PAGE_SHIFT - 4);  /* convert the addr to 4k page */
+
+	/* chip select are bits 14 & 13 */
+	row = pvt->mc_symmetric ? ((page >> 1) & 3) :
 		edac_mc_find_csrow_by_page(mci, page);
+
 	e752x_mc_printk(mci, KERN_WARNING,
 			"CE page 0x%lx, row %d : Memory read retry\n",
 			(long unsigned int)page, row);
@@ -422,12 +426,21 @@ static inline void process_threshold_ce(struct mem_ctl_info *mci, u16 error,
 }
 
 static char *global_message[11] = {
-	"PCI Express C1", "PCI Express C", "PCI Express B1",
-	"PCI Express B", "PCI Express A1", "PCI Express A",
-	"DMA Controler", "HUB or NS Interface", "System Bus",
-	"DRAM Controler", "Internal Buffer"
+	"PCI Express C1",
+	"PCI Express C",
+	"PCI Express B1",
+	"PCI Express B",
+	"PCI Express A1",
+	"PCI Express A",
+	"DMA Controller",
+	"HUB or NS Interface",
+	"System Bus",
+	"DRAM Controller",  /* 9th entry */
+	"Internal Buffer"
 };
 
+#define DRAM_ENTRY	9
+
 static char *fatal_message[2] = { "Non-Fatal ", "Fatal " };
 
 static void do_global_error(int fatal, u32 errors)
@@ -435,9 +448,16 @@ static void do_global_error(int fatal, u32 errors)
 	int i;
 
 	for (i = 0; i < 11; i++) {
-		if (errors & (1 << i))
-			e752x_printk(KERN_WARNING, "%sError %s\n",
-				fatal_message[fatal], global_message[i]);
+		if (errors & (1 << i)) {
+			/* If the error is from DRAM Controller OR
+			 * we are to report ALL errors, then
+			 * report the error
+			 */
+			if ((i == DRAM_ENTRY) || report_non_memory_errors)
+				e752x_printk(KERN_WARNING, "%sError %s\n",
+					fatal_message[fatal],
+					global_message[i]);
+		}
 	}
 }
 
@@ -1021,7 +1041,7 @@ static int e752x_get_devs(struct pci_dev *pdev, int dev_idx,
 	struct pci_dev *dev;
 
 	pvt->bridge_ck = pci_get_device(PCI_VENDOR_ID_INTEL,
-					pvt->dev_info->err_dev, pvt->bridge_ck);
+				pvt->dev_info->err_dev, pvt->bridge_ck);
 
 	if (pvt->bridge_ck == NULL)
 		pvt->bridge_ck = pci_scan_single_device(pdev->bus,
@@ -1034,8 +1054,9 @@ static int e752x_get_devs(struct pci_dev *pdev, int dev_idx,
 		return 1;
 	}
 
-	dev = pci_get_device(PCI_VENDOR_ID_INTEL, e752x_devs[dev_idx].ctl_dev,
-			NULL);
+	dev = pci_get_device(PCI_VENDOR_ID_INTEL,
+				e752x_devs[dev_idx].ctl_dev,
+				NULL);
 
 	if (dev == NULL)
 		goto fail;
@@ -1316,7 +1337,8 @@ MODULE_DESCRIPTION("MC support for Intel e752x/3100 memory controllers");
 
 module_param(force_function_unhide, int, 0444);
 MODULE_PARM_DESC(force_function_unhide, "if BIOS sets Dev0:Fun1 up as hidden:"
-		 " 1=force unhide and hope BIOS doesn't fight driver for Dev0:Fun1 access");
+		 " 1=force unhide and hope BIOS doesn't fight driver for "
+		"Dev0:Fun1 access");
 
 module_param(edac_op_state, int, 0444);
 MODULE_PARM_DESC(edac_op_state, "EDAC Error Reporting state: 0=Poll,1=NMI");
@@ -1324,3 +1346,6 @@ MODULE_PARM_DESC(edac_op_state, "EDAC Error Reporting state: 0=Poll,1=NMI");
 module_param(sysbus_parity, int, 0444);
 MODULE_PARM_DESC(sysbus_parity, "0=disable system bus parity checking,"
 		" 1=enable system bus parity checking, default=auto-detect");
+module_param(report_non_memory_errors, int, 0644);
+MODULE_PARM_DESC(report_non_memory_errors, "0=disable non-memory error "
+		"reporting, 1=enable non-memory error reporting");
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index 021d1879514..ad218fe4942 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -44,6 +44,25 @@ int edac_mc_get_poll_msec(void)
 	return edac_mc_poll_msec;
 }
 
+static int edac_set_poll_msec(const char *val, struct kernel_param *kp)
+{
+	long l;
+	int ret;
+
+	if (!val)
+		return -EINVAL;
+
+	ret = strict_strtol(val, 0, &l);
+	if (ret == -EINVAL || ((int)l != l))
+		return -EINVAL;
+	*((int *)kp->arg) = l;
+
+	/* notify edac_mc engine to reset the poll period */
+	edac_mc_reset_delay_period(l);
+
+	return 0;
+}
+
 /* Parameter declarations for above */
 module_param(edac_mc_panic_on_ue, int, 0644);
 MODULE_PARM_DESC(edac_mc_panic_on_ue, "Panic on uncorrected error: 0=off 1=on");
@@ -53,7 +72,8 @@ MODULE_PARM_DESC(edac_mc_log_ue,
 module_param(edac_mc_log_ce, int, 0644);
 MODULE_PARM_DESC(edac_mc_log_ce,
 		 "Log correctable error to console: 0=off 1=on");
-module_param(edac_mc_poll_msec, int, 0644);
+module_param_call(edac_mc_poll_msec, edac_set_poll_msec, param_get_int,
+		  &edac_mc_poll_msec, 0644);
 MODULE_PARM_DESC(edac_mc_poll_msec, "Polling period in milliseconds");
 
 /*
@@ -103,16 +123,6 @@ static const char *edac_caps[] = {
 
 
 
-/*
- * /sys/devices/system/edac/mc;
- *	data structures and methods
- */
-static ssize_t memctrl_int_show(void *ptr, char *buffer)
-{
-	int *value = (int *)ptr;
-	return sprintf(buffer, "%u\n", *value);
-}
-
 static ssize_t memctrl_int_store(void *ptr, const char *buffer, size_t count)
 {
 	int *value = (int *)ptr;
@@ -123,23 +133,6 @@ static ssize_t memctrl_int_store(void *ptr, const char *buffer, size_t count)
 	return count;
 }
 
-/*
- * mc poll_msec time value
- */
-static ssize_t poll_msec_int_store(void *ptr, const char *buffer, size_t count)
-{
-	int *value = (int *)ptr;
-
-	if (isdigit(*buffer)) {
-		*value = simple_strtoul(buffer, NULL, 0);
-
-		/* notify edac_mc engine to reset the poll period */
-		edac_mc_reset_delay_period(*value);
-	}
-
-	return count;
-}
-
 
 /* EDAC sysfs CSROW data structures and methods
  */
@@ -185,7 +178,11 @@ static ssize_t csrow_edac_mode_show(struct csrow_info *csrow, char *data,
 static ssize_t channel_dimm_label_show(struct csrow_info *csrow,
 				char *data, int channel)
 {
-	return snprintf(data, EDAC_MC_LABEL_LEN, "%s",
+	/* if field has not been initialized, there is nothing to send */
+	if (!csrow->channels[channel].label[0])
+		return 0;
+
+	return snprintf(data, EDAC_MC_LABEL_LEN, "%s\n",
 			csrow->channels[channel].label);
 }
 
@@ -649,98 +646,10 @@ static struct kobj_type ktype_mci = {
 	.default_attrs = (struct attribute **)mci_attr,
 };
 
-/* show/store, tables, etc for the MC kset */
-
-
-struct memctrl_dev_attribute {
-	struct attribute attr;
-	void *value;
-	 ssize_t(*show) (void *, char *);
-	 ssize_t(*store) (void *, const char *, size_t);
-};
-
-/* Set of show/store abstract level functions for memory control object */
-static ssize_t memctrl_dev_show(struct kobject *kobj,
-				struct attribute *attr, char *buffer)
-{
-	struct memctrl_dev_attribute *memctrl_dev;
-	memctrl_dev = (struct memctrl_dev_attribute *)attr;
-
-	if (memctrl_dev->show)
-		return memctrl_dev->show(memctrl_dev->value, buffer);
-
-	return -EIO;
-}
-
-static ssize_t memctrl_dev_store(struct kobject *kobj, struct attribute *attr,
-				 const char *buffer, size_t count)
-{
-	struct memctrl_dev_attribute *memctrl_dev;
-	memctrl_dev = (struct memctrl_dev_attribute *)attr;
-
-	if (memctrl_dev->store)
-		return memctrl_dev->store(memctrl_dev->value, buffer, count);
-
-	return -EIO;
-}
-
-static struct sysfs_ops memctrlfs_ops = {
-	.show = memctrl_dev_show,
-	.store = memctrl_dev_store
-};
-
-#define MEMCTRL_ATTR(_name, _mode, _show, _store)			\
-static struct memctrl_dev_attribute attr_##_name = {			\
-	.attr = {.name = __stringify(_name), .mode = _mode },	\
-	.value  = &_name,					\
-	.show   = _show,					\
-	.store  = _store,					\
-};
-
-#define MEMCTRL_STRING_ATTR(_name, _data, _mode, _show, _store)	\
-static struct memctrl_dev_attribute attr_##_name = {			\
-	.attr = {.name = __stringify(_name), .mode = _mode },	\
-	.value  = _data,					\
-	.show   = _show,					\
-	.store  = _store,					\
-};
-
-/* csrow<id> control files */
-MEMCTRL_ATTR(edac_mc_panic_on_ue,
-	S_IRUGO | S_IWUSR, memctrl_int_show, memctrl_int_store);
-
-MEMCTRL_ATTR(edac_mc_log_ue,
-	S_IRUGO | S_IWUSR, memctrl_int_show, memctrl_int_store);
-
-MEMCTRL_ATTR(edac_mc_log_ce,
-	S_IRUGO | S_IWUSR, memctrl_int_show, memctrl_int_store);
-
-MEMCTRL_ATTR(edac_mc_poll_msec,
-	S_IRUGO | S_IWUSR, memctrl_int_show, poll_msec_int_store);
-
-/* Base Attributes of the memory ECC object */
-static struct memctrl_dev_attribute *memctrl_attr[] = {
-	&attr_edac_mc_panic_on_ue,
-	&attr_edac_mc_log_ue,
-	&attr_edac_mc_log_ce,
-	&attr_edac_mc_poll_msec,
-	NULL,
-};
-
-
-/* the ktype for the mc_kset internal kobj */
-static struct kobj_type ktype_mc_set_attribs = {
-	.sysfs_ops = &memctrlfs_ops,
-	.default_attrs = (struct attribute **)memctrl_attr,
-};
-
 /* EDAC memory controller sysfs kset:
  *	/sys/devices/system/edac/mc
  */
-static struct kset mc_kset = {
-	.kobj = {.ktype = &ktype_mc_set_attribs },
-};
-
+static struct kset *mc_kset;
 
 /*
  * edac_mc_register_sysfs_main_kobj
@@ -771,7 +680,7 @@ int edac_mc_register_sysfs_main_kobj(struct mem_ctl_info *mci)
 	}
 
 	/* this instance become part of the mc_kset */
-	kobj_mci->kset = &mc_kset;
+	kobj_mci->kset = mc_kset;
 
 	/* register the mc<id> kobject to the mc_kset */
 	err = kobject_init_and_add(kobj_mci, &ktype_mci, NULL,
@@ -1001,12 +910,9 @@ int edac_sysfs_setup_mc_kset(void)
 	}
 
 	/* Init the MC's kobject */
-	kobject_set_name(&mc_kset.kobj, "mc");
-	mc_kset.kobj.parent = &edac_class->kset.kobj;
-
-	/* register the mc_kset */
-	err = kset_register(&mc_kset);
-	if (err) {
+	mc_kset = kset_create_and_add("mc", NULL, &edac_class->kset.kobj);
+	if (!mc_kset) {
+		err = -ENOMEM;
 		debugf1("%s() Failed to register '.../edac/mc'\n", __func__);
 		goto fail_out;
 	}
@@ -1028,6 +934,6 @@ fail_out:
  */
 void edac_sysfs_teardown_mc_kset(void)
 {
-	kset_unregister(&mc_kset);
+	kset_unregister(mc_kset);
 }
 
diff --git a/drivers/edac/edac_pci_sysfs.c b/drivers/edac/edac_pci_sysfs.c
index 2c1fa1bb6df..5c153dccc95 100644
--- a/drivers/edac/edac_pci_sysfs.c
+++ b/drivers/edac/edac_pci_sysfs.c
@@ -28,7 +28,7 @@ static int edac_pci_poll_msec = 1000;	/* one second workq period */
 static atomic_t pci_parity_count = ATOMIC_INIT(0);
 static atomic_t pci_nonparity_count = ATOMIC_INIT(0);
 
-static struct kobject edac_pci_top_main_kobj;
+static struct kobject *edac_pci_top_main_kobj;
 static atomic_t edac_pci_sysfs_refcount = ATOMIC_INIT(0);
 
 /* getter functions for the data variables */
@@ -83,7 +83,7 @@ static void edac_pci_instance_release(struct kobject *kobj)
 	pci = to_instance(kobj);
 
 	/* decrement reference count on top main kobj */
-	kobject_put(&edac_pci_top_main_kobj);
+	kobject_put(edac_pci_top_main_kobj);
 
 	kfree(pci);	/* Free the control struct */
 }
@@ -166,7 +166,7 @@ static int edac_pci_create_instance_kobj(struct edac_pci_ctl_info *pci, int idx)
 	 * track the number of PCI instances we have, and thus nest
 	 * properly on keeping the module loaded
 	 */
-	main_kobj = kobject_get(&edac_pci_top_main_kobj);
+	main_kobj = kobject_get(edac_pci_top_main_kobj);
 	if (!main_kobj) {
 		err = -ENODEV;
 		goto error_out;
@@ -174,11 +174,11 @@ static int edac_pci_create_instance_kobj(struct edac_pci_ctl_info *pci, int idx)
 
 	/* And now register this new kobject under the main kobj */
 	err = kobject_init_and_add(&pci->kobj, &ktype_pci_instance,
-				   &edac_pci_top_main_kobj, "pci%d", idx);
+				   edac_pci_top_main_kobj, "pci%d", idx);
 	if (err != 0) {
 		debugf2("%s() failed to register instance pci%d\n",
 			__func__, idx);
-		kobject_put(&edac_pci_top_main_kobj);
+		kobject_put(edac_pci_top_main_kobj);
 		goto error_out;
 	}
 
@@ -316,9 +316,10 @@ static struct edac_pci_dev_attribute *edac_pci_attr[] = {
  */
 static void edac_pci_release_main_kobj(struct kobject *kobj)
 {
-
 	debugf0("%s() here to module_put(THIS_MODULE)\n", __func__);
 
+	kfree(kobj);
+
 	/* last reference to top EDAC PCI kobject has been removed,
 	 * NOW release our ref count on the core module
 	 */
@@ -369,8 +370,16 @@ static int edac_pci_main_kobj_setup(void)
 		goto decrement_count_fail;
 	}
 
+	edac_pci_top_main_kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
+	if (!edac_pci_top_main_kobj) {
+		debugf1("Failed to allocate\n");
+		err = -ENOMEM;
+		goto kzalloc_fail;
+	}
+
 	/* Instanstiate the pci object */
-	err = kobject_init_and_add(&edac_pci_top_main_kobj, &ktype_edac_pci_main_kobj,
+	err = kobject_init_and_add(edac_pci_top_main_kobj,
+				   &ktype_edac_pci_main_kobj,
 				   &edac_class->kset.kobj, "pci");
 	if (err) {
 		debugf1("Failed to register '.../edac/pci'\n");
@@ -381,13 +390,16 @@ static int edac_pci_main_kobj_setup(void)
 	 * for EDAC PCI, then edac_pci_main_kobj_teardown()
 	 * must be used, for resources to be cleaned up properly
 	 */
-	kobject_uevent(&edac_pci_top_main_kobj, KOBJ_ADD);
+	kobject_uevent(edac_pci_top_main_kobj, KOBJ_ADD);
 	debugf1("Registered '.../edac/pci' kobject\n");
 
 	return 0;
 
 	/* Error unwind statck */
 kobject_init_and_add_fail:
+	kfree(edac_pci_top_main_kobj);
+
+kzalloc_fail:
 	module_put(THIS_MODULE);
 
 decrement_count_fail:
@@ -414,7 +426,7 @@ static void edac_pci_main_kobj_teardown(void)
 	if (atomic_dec_return(&edac_pci_sysfs_refcount) == 0) {
 		debugf0("%s() called kobject_put on main kobj\n",
 			__func__);
-		kobject_put(&edac_pci_top_main_kobj);
+		kobject_put(edac_pci_top_main_kobj);
 	}
 }
 
diff --git a/drivers/edac/i5100_edac.c b/drivers/edac/i5100_edac.c
new file mode 100644
index 00000000000..22db05a67bf
--- /dev/null
+++ b/drivers/edac/i5100_edac.c
@@ -0,0 +1,981 @@
+/*
+ * Intel 5100 Memory Controllers kernel module
+ *
+ * This file may be distributed under the terms of the
+ * GNU General Public License.
+ *
+ * This module is based on the following document:
+ *
+ * Intel 5100X Chipset Memory Controller Hub (MCH) - Datasheet
+ *      http://download.intel.com/design/chipsets/datashts/318378.pdf
+ *
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/slab.h>
+#include <linux/edac.h>
+#include <linux/delay.h>
+#include <linux/mmzone.h>
+
+#include "edac_core.h"
+
+/* register addresses */
+
+/* device 16, func 1 */
+#define I5100_MC		0x40	/* Memory Control Register */
+#define I5100_MS		0x44	/* Memory Status Register */
+#define I5100_SPDDATA		0x48	/* Serial Presence Detect Status Reg */
+#define I5100_SPDCMD		0x4c	/* Serial Presence Detect Command Reg */
+#define I5100_TOLM		0x6c	/* Top of Low Memory */
+#define I5100_MIR0		0x80	/* Memory Interleave Range 0 */
+#define I5100_MIR1		0x84	/* Memory Interleave Range 1 */
+#define I5100_AMIR_0		0x8c	/* Adjusted Memory Interleave Range 0 */
+#define I5100_AMIR_1		0x90	/* Adjusted Memory Interleave Range 1 */
+#define I5100_FERR_NF_MEM	0xa0	/* MC First Non Fatal Errors */
+#define		I5100_FERR_NF_MEM_M16ERR_MASK	(1 << 16)
+#define		I5100_FERR_NF_MEM_M15ERR_MASK	(1 << 15)
+#define		I5100_FERR_NF_MEM_M14ERR_MASK	(1 << 14)
+#define		I5100_FERR_NF_MEM_M12ERR_MASK	(1 << 12)
+#define		I5100_FERR_NF_MEM_M11ERR_MASK	(1 << 11)
+#define		I5100_FERR_NF_MEM_M10ERR_MASK	(1 << 10)
+#define		I5100_FERR_NF_MEM_M6ERR_MASK	(1 << 6)
+#define		I5100_FERR_NF_MEM_M5ERR_MASK	(1 << 5)
+#define		I5100_FERR_NF_MEM_M4ERR_MASK	(1 << 4)
+#define		I5100_FERR_NF_MEM_M1ERR_MASK	1
+#define		I5100_FERR_NF_MEM_ANY_MASK	\
+			(I5100_FERR_NF_MEM_M16ERR_MASK | \
+			I5100_FERR_NF_MEM_M15ERR_MASK | \
+			I5100_FERR_NF_MEM_M14ERR_MASK | \
+			I5100_FERR_NF_MEM_M12ERR_MASK | \
+			I5100_FERR_NF_MEM_M11ERR_MASK | \
+			I5100_FERR_NF_MEM_M10ERR_MASK | \
+			I5100_FERR_NF_MEM_M6ERR_MASK | \
+			I5100_FERR_NF_MEM_M5ERR_MASK | \
+			I5100_FERR_NF_MEM_M4ERR_MASK | \
+			I5100_FERR_NF_MEM_M1ERR_MASK)
+#define	I5100_NERR_NF_MEM	0xa4	/* MC Next Non-Fatal Errors */
+#define I5100_EMASK_MEM		0xa8	/* MC Error Mask Register */
+
+/* device 21 and 22, func 0 */
+#define I5100_MTR_0	0x154	/* Memory Technology Registers 0-3 */
+#define I5100_DMIR	0x15c	/* DIMM Interleave Range */
+#define	I5100_VALIDLOG	0x18c	/* Valid Log Markers */
+#define	I5100_NRECMEMA	0x190	/* Non-Recoverable Memory Error Log Reg A */
+#define	I5100_NRECMEMB	0x194	/* Non-Recoverable Memory Error Log Reg B */
+#define	I5100_REDMEMA	0x198	/* Recoverable Memory Data Error Log Reg A */
+#define	I5100_REDMEMB	0x19c	/* Recoverable Memory Data Error Log Reg B */
+#define	I5100_RECMEMA	0x1a0	/* Recoverable Memory Error Log Reg A */
+#define	I5100_RECMEMB	0x1a4	/* Recoverable Memory Error Log Reg B */
+#define I5100_MTR_4	0x1b0	/* Memory Technology Registers 4,5 */
+
+/* bit field accessors */
+
+static inline u32 i5100_mc_errdeten(u32 mc)
+{
+	return mc >> 5 & 1;
+}
+
+static inline u16 i5100_spddata_rdo(u16 a)
+{
+	return a >> 15 & 1;
+}
+
+static inline u16 i5100_spddata_sbe(u16 a)
+{
+	return a >> 13 & 1;
+}
+
+static inline u16 i5100_spddata_busy(u16 a)
+{
+	return a >> 12 & 1;
+}
+
+static inline u16 i5100_spddata_data(u16 a)
+{
+	return a & ((1 << 8) - 1);
+}
+
+static inline u32 i5100_spdcmd_create(u32 dti, u32 ckovrd, u32 sa, u32 ba,
+				      u32 data, u32 cmd)
+{
+	return	((dti & ((1 << 4) - 1))  << 28) |
+		((ckovrd & 1)            << 27) |
+		((sa & ((1 << 3) - 1))   << 24) |
+		((ba & ((1 << 8) - 1))   << 16) |
+		((data & ((1 << 8) - 1)) <<  8) |
+		(cmd & 1);
+}
+
+static inline u16 i5100_tolm_tolm(u16 a)
+{
+	return a >> 12 & ((1 << 4) - 1);
+}
+
+static inline u16 i5100_mir_limit(u16 a)
+{
+	return a >> 4 & ((1 << 12) - 1);
+}
+
+static inline u16 i5100_mir_way1(u16 a)
+{
+	return a >> 1 & 1;
+}
+
+static inline u16 i5100_mir_way0(u16 a)
+{
+	return a & 1;
+}
+
+static inline u32 i5100_ferr_nf_mem_chan_indx(u32 a)
+{
+	return a >> 28 & 1;
+}
+
+static inline u32 i5100_ferr_nf_mem_any(u32 a)
+{
+	return a & I5100_FERR_NF_MEM_ANY_MASK;
+}
+
+static inline u32 i5100_nerr_nf_mem_any(u32 a)
+{
+	return i5100_ferr_nf_mem_any(a);
+}
+
+static inline u32 i5100_dmir_limit(u32 a)
+{
+	return a >> 16 & ((1 << 11) - 1);
+}
+
+static inline u32 i5100_dmir_rank(u32 a, u32 i)
+{
+	return a >> (4 * i) & ((1 << 2) - 1);
+}
+
+static inline u16 i5100_mtr_present(u16 a)
+{
+	return a >> 10 & 1;
+}
+
+static inline u16 i5100_mtr_ethrottle(u16 a)
+{
+	return a >> 9 & 1;
+}
+
+static inline u16 i5100_mtr_width(u16 a)
+{
+	return a >> 8 & 1;
+}
+
+static inline u16 i5100_mtr_numbank(u16 a)
+{
+	return a >> 6 & 1;
+}
+
+static inline u16 i5100_mtr_numrow(u16 a)
+{
+	return a >> 2 & ((1 << 2) - 1);
+}
+
+static inline u16 i5100_mtr_numcol(u16 a)
+{
+	return a & ((1 << 2) - 1);
+}
+
+
+static inline u32 i5100_validlog_redmemvalid(u32 a)
+{
+	return a >> 2 & 1;
+}
+
+static inline u32 i5100_validlog_recmemvalid(u32 a)
+{
+	return a >> 1 & 1;
+}
+
+static inline u32 i5100_validlog_nrecmemvalid(u32 a)
+{
+	return a & 1;
+}
+
+static inline u32 i5100_nrecmema_merr(u32 a)
+{
+	return a >> 15 & ((1 << 5) - 1);
+}
+
+static inline u32 i5100_nrecmema_bank(u32 a)
+{
+	return a >> 12 & ((1 << 3) - 1);
+}
+
+static inline u32 i5100_nrecmema_rank(u32 a)
+{
+	return a >>  8 & ((1 << 3) - 1);
+}
+
+static inline u32 i5100_nrecmema_dm_buf_id(u32 a)
+{
+	return a & ((1 << 8) - 1);
+}
+
+static inline u32 i5100_nrecmemb_cas(u32 a)
+{
+	return a >> 16 & ((1 << 13) - 1);
+}
+
+static inline u32 i5100_nrecmemb_ras(u32 a)
+{
+	return a & ((1 << 16) - 1);
+}
+
+static inline u32 i5100_redmemb_ecc_locator(u32 a)
+{
+	return a & ((1 << 18) - 1);
+}
+
+static inline u32 i5100_recmema_merr(u32 a)
+{
+	return i5100_nrecmema_merr(a);
+}
+
+static inline u32 i5100_recmema_bank(u32 a)
+{
+	return i5100_nrecmema_bank(a);
+}
+
+static inline u32 i5100_recmema_rank(u32 a)
+{
+	return i5100_nrecmema_rank(a);
+}
+
+static inline u32 i5100_recmema_dm_buf_id(u32 a)
+{
+	return i5100_nrecmema_dm_buf_id(a);
+}
+
+static inline u32 i5100_recmemb_cas(u32 a)
+{
+	return i5100_nrecmemb_cas(a);
+}
+
+static inline u32 i5100_recmemb_ras(u32 a)
+{
+	return i5100_nrecmemb_ras(a);
+}
+
+/* some generic limits */
+#define I5100_MAX_RANKS_PER_CTLR	6
+#define I5100_MAX_CTLRS			2
+#define I5100_MAX_RANKS_PER_DIMM	4
+#define I5100_DIMM_ADDR_LINES		(6 - 3)	/* 64 bits / 8 bits per byte */
+#define I5100_MAX_DIMM_SLOTS_PER_CTLR	4
+#define I5100_MAX_RANK_INTERLEAVE	4
+#define I5100_MAX_DMIRS			5
+
+struct i5100_priv {
+	/* ranks on each dimm -- 0 maps to not present -- obtained via SPD */
+	int dimm_numrank[I5100_MAX_CTLRS][I5100_MAX_DIMM_SLOTS_PER_CTLR];
+
+	/*
+	 * mainboard chip select map -- maps i5100 chip selects to
+	 * DIMM slot chip selects.  In the case of only 4 ranks per
+	 * controller, the mapping is fairly obvious but not unique.
+	 * we map -1 -> NC and assume both controllers use the same
+	 * map...
+	 *
+	 */
+	int dimm_csmap[I5100_MAX_DIMM_SLOTS_PER_CTLR][I5100_MAX_RANKS_PER_DIMM];
+
+	/* memory interleave range */
+	struct {
+		u64	 limit;
+		unsigned way[2];
+	} mir[I5100_MAX_CTLRS];
+
+	/* adjusted memory interleave range register */
+	unsigned amir[I5100_MAX_CTLRS];
+
+	/* dimm interleave range */
+	struct {
+		unsigned rank[I5100_MAX_RANK_INTERLEAVE];
+		u64	 limit;
+	} dmir[I5100_MAX_CTLRS][I5100_MAX_DMIRS];
+
+	/* memory technology registers... */
+	struct {
+		unsigned present;	/* 0 or 1 */
+		unsigned ethrottle;	/* 0 or 1 */
+		unsigned width;		/* 4 or 8 bits  */
+		unsigned numbank;	/* 2 or 3 lines */
+		unsigned numrow;	/* 13 .. 16 lines */
+		unsigned numcol;	/* 11 .. 12 lines */
+	} mtr[I5100_MAX_CTLRS][I5100_MAX_RANKS_PER_CTLR];
+
+	u64 tolm;		/* top of low memory in bytes */
+	unsigned ranksperctlr;	/* number of ranks per controller */
+
+	struct pci_dev *mc;	/* device 16 func 1 */
+	struct pci_dev *ch0mm;	/* device 21 func 0 */
+	struct pci_dev *ch1mm;	/* device 22 func 0 */
+};
+
+/* map a rank/ctlr to a slot number on the mainboard */
+static int i5100_rank_to_slot(const struct mem_ctl_info *mci,
+			      int ctlr, int rank)
+{
+	const struct i5100_priv *priv = mci->pvt_info;
+	int i;
+
+	for (i = 0; i < I5100_MAX_DIMM_SLOTS_PER_CTLR; i++) {
+		int j;
+		const int numrank = priv->dimm_numrank[ctlr][i];
+
+		for (j = 0; j < numrank; j++)
+			if (priv->dimm_csmap[i][j] == rank)
+				return i * 2 + ctlr;
+	}
+
+	return -1;
+}
+
+static const char *i5100_err_msg(unsigned err)
+{
+	static const char *merrs[] = {
+		"unknown", /* 0 */
+		"uncorrectable data ECC on replay", /* 1 */
+		"unknown", /* 2 */
+		"unknown", /* 3 */
+		"aliased uncorrectable demand data ECC", /* 4 */
+		"aliased uncorrectable spare-copy data ECC", /* 5 */
+		"aliased uncorrectable patrol data ECC", /* 6 */
+		"unknown", /* 7 */
+		"unknown", /* 8 */
+		"unknown", /* 9 */
+		"non-aliased uncorrectable demand data ECC", /* 10 */
+		"non-aliased uncorrectable spare-copy data ECC", /* 11 */
+		"non-aliased uncorrectable patrol data ECC", /* 12 */
+		"unknown", /* 13 */
+		"correctable demand data ECC", /* 14 */
+		"correctable spare-copy data ECC", /* 15 */
+		"correctable patrol data ECC", /* 16 */
+		"unknown", /* 17 */
+		"SPD protocol error", /* 18 */
+		"unknown", /* 19 */
+		"spare copy initiated", /* 20 */
+		"spare copy completed", /* 21 */
+	};
+	unsigned i;
+
+	for (i = 0; i < ARRAY_SIZE(merrs); i++)
+		if (1 << i & err)
+			return merrs[i];
+
+	return "none";
+}
+
+/* convert csrow index into a rank (per controller -- 0..5) */
+static int i5100_csrow_to_rank(const struct mem_ctl_info *mci, int csrow)
+{
+	const struct i5100_priv *priv = mci->pvt_info;
+
+	return csrow % priv->ranksperctlr;
+}
+
+/* convert csrow index into a controller (0..1) */
+static int i5100_csrow_to_cntlr(const struct mem_ctl_info *mci, int csrow)
+{
+	const struct i5100_priv *priv = mci->pvt_info;
+
+	return csrow / priv->ranksperctlr;
+}
+
+static unsigned i5100_rank_to_csrow(const struct mem_ctl_info *mci,
+				    int ctlr, int rank)
+{
+	const struct i5100_priv *priv = mci->pvt_info;
+
+	return ctlr * priv->ranksperctlr + rank;
+}
+
+static void i5100_handle_ce(struct mem_ctl_info *mci,
+			    int ctlr,
+			    unsigned bank,
+			    unsigned rank,
+			    unsigned long syndrome,
+			    unsigned cas,
+			    unsigned ras,
+			    const char *msg)
+{
+	const int csrow = i5100_rank_to_csrow(mci, ctlr, rank);
+
+	printk(KERN_ERR
+		"CE ctlr %d, bank %u, rank %u, syndrome 0x%lx, "
+		"cas %u, ras %u, csrow %u, label \"%s\": %s\n",
+		ctlr, bank, rank, syndrome, cas, ras,
+		csrow, mci->csrows[csrow].channels[0].label, msg);
+
+	mci->ce_count++;
+	mci->csrows[csrow].ce_count++;
+	mci->csrows[csrow].channels[0].ce_count++;
+}
+
+static void i5100_handle_ue(struct mem_ctl_info *mci,
+			    int ctlr,
+			    unsigned bank,
+			    unsigned rank,
+			    unsigned long syndrome,
+			    unsigned cas,
+			    unsigned ras,
+			    const char *msg)
+{
+	const int csrow = i5100_rank_to_csrow(mci, ctlr, rank);
+
+	printk(KERN_ERR
+		"UE ctlr %d, bank %u, rank %u, syndrome 0x%lx, "
+		"cas %u, ras %u, csrow %u, label \"%s\": %s\n",
+		ctlr, bank, rank, syndrome, cas, ras,
+		csrow, mci->csrows[csrow].channels[0].label, msg);
+
+	mci->ue_count++;
+	mci->csrows[csrow].ue_count++;
+}
+
+static void i5100_read_log(struct mem_ctl_info *mci, int ctlr,
+			   u32 ferr, u32 nerr)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	struct pci_dev *pdev = (ctlr) ? priv->ch1mm : priv->ch0mm;
+	u32 dw;
+	u32 dw2;
+	unsigned syndrome = 0;
+	unsigned ecc_loc = 0;
+	unsigned merr;
+	unsigned bank;
+	unsigned rank;
+	unsigned cas;
+	unsigned ras;
+
+	pci_read_config_dword(pdev, I5100_VALIDLOG, &dw);
+
+	if (i5100_validlog_redmemvalid(dw)) {
+		pci_read_config_dword(pdev, I5100_REDMEMA, &dw2);
+		syndrome = dw2;
+		pci_read_config_dword(pdev, I5100_REDMEMB, &dw2);
+		ecc_loc = i5100_redmemb_ecc_locator(dw2);
+	}
+
+	if (i5100_validlog_recmemvalid(dw)) {
+		const char *msg;
+
+		pci_read_config_dword(pdev, I5100_RECMEMA, &dw2);
+		merr = i5100_recmema_merr(dw2);
+		bank = i5100_recmema_bank(dw2);
+		rank = i5100_recmema_rank(dw2);
+
+		pci_read_config_dword(pdev, I5100_RECMEMB, &dw2);
+		cas = i5100_recmemb_cas(dw2);
+		ras = i5100_recmemb_ras(dw2);
+
+		/* FIXME:  not really sure if this is what merr is...
+		 */
+		if (!merr)
+			msg = i5100_err_msg(ferr);
+		else
+			msg = i5100_err_msg(nerr);
+
+		i5100_handle_ce(mci, ctlr, bank, rank, syndrome, cas, ras, msg);
+	}
+
+	if (i5100_validlog_nrecmemvalid(dw)) {
+		const char *msg;
+
+		pci_read_config_dword(pdev, I5100_NRECMEMA, &dw2);
+		merr = i5100_nrecmema_merr(dw2);
+		bank = i5100_nrecmema_bank(dw2);
+		rank = i5100_nrecmema_rank(dw2);
+
+		pci_read_config_dword(pdev, I5100_NRECMEMB, &dw2);
+		cas = i5100_nrecmemb_cas(dw2);
+		ras = i5100_nrecmemb_ras(dw2);
+
+		/* FIXME:  not really sure if this is what merr is...
+		 */
+		if (!merr)
+			msg = i5100_err_msg(ferr);
+		else
+			msg = i5100_err_msg(nerr);
+
+		i5100_handle_ue(mci, ctlr, bank, rank, syndrome, cas, ras, msg);
+	}
+
+	pci_write_config_dword(pdev, I5100_VALIDLOG, dw);
+}
+
+static void i5100_check_error(struct mem_ctl_info *mci)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	u32 dw;
+
+
+	pci_read_config_dword(priv->mc, I5100_FERR_NF_MEM, &dw);
+	if (i5100_ferr_nf_mem_any(dw)) {
+		u32 dw2;
+
+		pci_read_config_dword(priv->mc, I5100_NERR_NF_MEM, &dw2);
+		if (dw2)
+			pci_write_config_dword(priv->mc, I5100_NERR_NF_MEM,
+					       dw2);
+		pci_write_config_dword(priv->mc, I5100_FERR_NF_MEM, dw);
+
+		i5100_read_log(mci, i5100_ferr_nf_mem_chan_indx(dw),
+			       i5100_ferr_nf_mem_any(dw),
+			       i5100_nerr_nf_mem_any(dw2));
+	}
+}
+
+static struct pci_dev *pci_get_device_func(unsigned vendor,
+					   unsigned device,
+					   unsigned func)
+{
+	struct pci_dev *ret = NULL;
+
+	while (1) {
+		ret = pci_get_device(vendor, device, ret);
+
+		if (!ret)
+			break;
+
+		if (PCI_FUNC(ret->devfn) == func)
+			break;
+	}
+
+	return ret;
+}
+
+static unsigned long __devinit i5100_npages(struct mem_ctl_info *mci,
+					    int csrow)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	const unsigned ctlr_rank = i5100_csrow_to_rank(mci, csrow);
+	const unsigned ctlr = i5100_csrow_to_cntlr(mci, csrow);
+	unsigned addr_lines;
+
+	/* dimm present? */
+	if (!priv->mtr[ctlr][ctlr_rank].present)
+		return 0ULL;
+
+	addr_lines =
+		I5100_DIMM_ADDR_LINES +
+		priv->mtr[ctlr][ctlr_rank].numcol +
+		priv->mtr[ctlr][ctlr_rank].numrow +
+		priv->mtr[ctlr][ctlr_rank].numbank;
+
+	return (unsigned long)
+		((unsigned long long) (1ULL << addr_lines) / PAGE_SIZE);
+}
+
+static void __devinit i5100_init_mtr(struct mem_ctl_info *mci)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	struct pci_dev *mms[2] = { priv->ch0mm, priv->ch1mm };
+	int i;
+
+	for (i = 0; i < I5100_MAX_CTLRS; i++) {
+		int j;
+		struct pci_dev *pdev = mms[i];
+
+		for (j = 0; j < I5100_MAX_RANKS_PER_CTLR; j++) {
+			const unsigned addr =
+				(j < 4) ? I5100_MTR_0 + j * 2 :
+					  I5100_MTR_4 + (j - 4) * 2;
+			u16 w;
+
+			pci_read_config_word(pdev, addr, &w);
+
+			priv->mtr[i][j].present = i5100_mtr_present(w);
+			priv->mtr[i][j].ethrottle = i5100_mtr_ethrottle(w);
+			priv->mtr[i][j].width = 4 + 4 * i5100_mtr_width(w);
+			priv->mtr[i][j].numbank = 2 + i5100_mtr_numbank(w);
+			priv->mtr[i][j].numrow = 13 + i5100_mtr_numrow(w);
+			priv->mtr[i][j].numcol = 10 + i5100_mtr_numcol(w);
+		}
+	}
+}
+
+/*
+ * FIXME: make this into a real i2c adapter (so that dimm-decode
+ * will work)?
+ */
+static int i5100_read_spd_byte(const struct mem_ctl_info *mci,
+			       u8 ch, u8 slot, u8 addr, u8 *byte)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	u16 w;
+	unsigned long et;
+
+	pci_read_config_word(priv->mc, I5100_SPDDATA, &w);
+	if (i5100_spddata_busy(w))
+		return -1;
+
+	pci_write_config_dword(priv->mc, I5100_SPDCMD,
+			       i5100_spdcmd_create(0xa, 1, ch * 4 + slot, addr,
+						   0, 0));
+
+	/* wait up to 100ms */
+	et = jiffies + HZ / 10;
+	udelay(100);
+	while (1) {
+		pci_read_config_word(priv->mc, I5100_SPDDATA, &w);
+		if (!i5100_spddata_busy(w))
+			break;
+		udelay(100);
+	}
+
+	if (!i5100_spddata_rdo(w) || i5100_spddata_sbe(w))
+		return -1;
+
+	*byte = i5100_spddata_data(w);
+
+	return 0;
+}
+
+/*
+ * fill dimm chip select map
+ *
+ * FIXME:
+ *   o only valid for 4 ranks per controller
+ *   o not the only way to may chip selects to dimm slots
+ *   o investigate if there is some way to obtain this map from the bios
+ */
+static void __devinit i5100_init_dimm_csmap(struct mem_ctl_info *mci)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	int i;
+
+	WARN_ON(priv->ranksperctlr != 4);
+
+	for (i = 0; i < I5100_MAX_DIMM_SLOTS_PER_CTLR; i++) {
+		int j;
+
+		for (j = 0; j < I5100_MAX_RANKS_PER_DIMM; j++)
+			priv->dimm_csmap[i][j] = -1; /* default NC */
+	}
+
+	/* only 2 chip selects per slot... */
+	priv->dimm_csmap[0][0] = 0;
+	priv->dimm_csmap[0][1] = 3;
+	priv->dimm_csmap[1][0] = 1;
+	priv->dimm_csmap[1][1] = 2;
+	priv->dimm_csmap[2][0] = 2;
+	priv->dimm_csmap[3][0] = 3;
+}
+
+static void __devinit i5100_init_dimm_layout(struct pci_dev *pdev,
+					     struct mem_ctl_info *mci)
+{
+	struct i5100_priv *priv = mci->pvt_info;
+	int i;
+
+	for (i = 0; i < I5100_MAX_CTLRS; i++) {
+		int j;
+
+		for (j = 0; j < I5100_MAX_DIMM_SLOTS_PER_CTLR; j++) {
+			u8 rank;
+
+			if (i5100_read_spd_byte(mci, i, j, 5, &rank) < 0)
+				priv->dimm_numrank[i][j] = 0;
+			else
+				priv->dimm_numrank[i][j] = (rank & 3) + 1;
+		}
+	}
+
+	i5100_init_dimm_csmap(mci);
+}
+
+static void __devinit i5100_init_interleaving(struct pci_dev *pdev,
+					      struct mem_ctl_info *mci)
+{
+	u16 w;
+	u32 dw;
+	struct i5100_priv *priv = mci->pvt_info;
+	struct pci_dev *mms[2] = { priv->ch0mm, priv->ch1mm };
+	int i;
+
+	pci_read_config_word(pdev, I5100_TOLM, &w);
+	priv->tolm = (u64) i5100_tolm_tolm(w) * 256 * 1024 * 1024;
+
+	pci_read_config_word(pdev, I5100_MIR0, &w);
+	priv->mir[0].limit = (u64) i5100_mir_limit(w) << 28;
+	priv->mir[0].way[1] = i5100_mir_way1(w);
+	priv->mir[0].way[0] = i5100_mir_way0(w);
+
+	pci_read_config_word(pdev, I5100_MIR1, &w);
+	priv->mir[1].limit = (u64) i5100_mir_limit(w) << 28;
+	priv->mir[1].way[1] = i5100_mir_way1(w);
+	priv->mir[1].way[0] = i5100_mir_way0(w);
+
+	pci_read_config_word(pdev, I5100_AMIR_0, &w);
+	priv->amir[0] = w;
+	pci_read_config_word(pdev, I5100_AMIR_1, &w);
+	priv->amir[1] = w;
+
+	for (i = 0; i < I5100_MAX_CTLRS; i++) {
+		int j;
+
+		for (j = 0; j < 5; j++) {
+			int k;
+
+			pci_read_config_dword(mms[i], I5100_DMIR + j * 4, &dw);
+
+			priv->dmir[i][j].limit =
+				(u64) i5100_dmir_limit(dw) << 28;
+			for (k = 0; k < I5100_MAX_RANKS_PER_DIMM; k++)
+				priv->dmir[i][j].rank[k] =
+					i5100_dmir_rank(dw, k);
+		}
+	}
+
+	i5100_init_mtr(mci);
+}
+
+static void __devinit i5100_init_csrows(struct mem_ctl_info *mci)
+{
+	int i;
+	unsigned long total_pages = 0UL;
+	struct i5100_priv *priv = mci->pvt_info;
+
+	for (i = 0; i < mci->nr_csrows; i++) {
+		const unsigned long npages = i5100_npages(mci, i);
+		const unsigned cntlr = i5100_csrow_to_cntlr(mci, i);
+		const unsigned rank = i5100_csrow_to_rank(mci, i);
+
+		if (!npages)
+			continue;
+
+		/*
+		 * FIXME: these two are totally bogus -- I don't see how to
+		 * map them correctly to this structure...
+		 */
+		mci->csrows[i].first_page = total_pages;
+		mci->csrows[i].last_page = total_pages + npages - 1;
+		mci->csrows[i].page_mask = 0UL;
+
+		mci->csrows[i].nr_pages = npages;
+		mci->csrows[i].grain = 32;
+		mci->csrows[i].csrow_idx = i;
+		mci->csrows[i].dtype =
+			(priv->mtr[cntlr][rank].width == 4) ? DEV_X4 : DEV_X8;
+		mci->csrows[i].ue_count = 0;
+		mci->csrows[i].ce_count = 0;
+		mci->csrows[i].mtype = MEM_RDDR2;
+		mci->csrows[i].edac_mode = EDAC_SECDED;
+		mci->csrows[i].mci = mci;
+		mci->csrows[i].nr_channels = 1;
+		mci->csrows[i].channels[0].chan_idx = 0;
+		mci->csrows[i].channels[0].ce_count = 0;
+		mci->csrows[i].channels[0].csrow = mci->csrows + i;
+		snprintf(mci->csrows[i].channels[0].label,
+			 sizeof(mci->csrows[i].channels[0].label),
+			 "DIMM%u", i5100_rank_to_slot(mci, cntlr, rank));
+
+		total_pages += npages;
+	}
+}
+
+static int __devinit i5100_init_one(struct pci_dev *pdev,
+				    const struct pci_device_id *id)
+{
+	int rc;
+	struct mem_ctl_info *mci;
+	struct i5100_priv *priv;
+	struct pci_dev *ch0mm, *ch1mm;
+	int ret = 0;
+	u32 dw;
+	int ranksperch;
+
+	if (PCI_FUNC(pdev->devfn) != 1)
+		return -ENODEV;
+
+	rc = pci_enable_device(pdev);
+	if (rc < 0) {
+		ret = rc;
+		goto bail;
+	}
+
+	/* ECC enabled? */
+	pci_read_config_dword(pdev, I5100_MC, &dw);
+	if (!i5100_mc_errdeten(dw)) {
+		printk(KERN_INFO "i5100_edac: ECC not enabled.\n");
+		ret = -ENODEV;
+		goto bail_pdev;
+	}
+
+	/* figure out how many ranks, from strapped state of 48GB_Mode input */
+	pci_read_config_dword(pdev, I5100_MS, &dw);
+	ranksperch = !!(dw & (1 << 8)) * 2 + 4;
+
+	if (ranksperch != 4) {
+		/* FIXME: get 6 ranks / controller to work - need hw... */
+		printk(KERN_INFO "i5100_edac: unsupported configuration.\n");
+		ret = -ENODEV;
+		goto bail_pdev;
+	}
+
+	/* enable error reporting... */
+	pci_read_config_dword(pdev, I5100_EMASK_MEM, &dw);
+	dw &= ~I5100_FERR_NF_MEM_ANY_MASK;
+	pci_write_config_dword(pdev, I5100_EMASK_MEM, dw);
+
+	/* device 21, func 0, Channel 0 Memory Map, Error Flag/Mask, etc... */
+	ch0mm = pci_get_device_func(PCI_VENDOR_ID_INTEL,
+				    PCI_DEVICE_ID_INTEL_5100_21, 0);
+	if (!ch0mm) {
+		ret = -ENODEV;
+		goto bail_pdev;
+	}
+
+	rc = pci_enable_device(ch0mm);
+	if (rc < 0) {
+		ret = rc;
+		goto bail_ch0;
+	}
+
+	/* device 22, func 0, Channel 1 Memory Map, Error Flag/Mask, etc... */
+	ch1mm = pci_get_device_func(PCI_VENDOR_ID_INTEL,
+				    PCI_DEVICE_ID_INTEL_5100_22, 0);
+	if (!ch1mm) {
+		ret = -ENODEV;
+		goto bail_disable_ch0;
+	}
+
+	rc = pci_enable_device(ch1mm);
+	if (rc < 0) {
+		ret = rc;
+		goto bail_ch1;
+	}
+
+	mci = edac_mc_alloc(sizeof(*priv), ranksperch * 2, 1, 0);
+	if (!mci) {
+		ret = -ENOMEM;
+		goto bail_disable_ch1;
+	}
+
+	mci->dev = &pdev->dev;
+
+	priv = mci->pvt_info;
+	priv->ranksperctlr = ranksperch;
+	priv->mc = pdev;
+	priv->ch0mm = ch0mm;
+	priv->ch1mm = ch1mm;
+
+	i5100_init_dimm_layout(pdev, mci);
+	i5100_init_interleaving(pdev, mci);
+
+	mci->mtype_cap = MEM_FLAG_FB_DDR2;
+	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
+	mci->edac_cap = EDAC_FLAG_SECDED;
+	mci->mod_name = "i5100_edac.c";
+	mci->mod_ver = "not versioned";
+	mci->ctl_name = "i5100";
+	mci->dev_name = pci_name(pdev);
+	mci->ctl_page_to_phys = NULL;
+
+	mci->edac_check = i5100_check_error;
+
+	i5100_init_csrows(mci);
+
+	/* this strange construction seems to be in every driver, dunno why */
+	switch (edac_op_state) {
+	case EDAC_OPSTATE_POLL:
+	case EDAC_OPSTATE_NMI:
+		break;
+	default:
+		edac_op_state = EDAC_OPSTATE_POLL;
+		break;
+	}
+
+	if (edac_mc_add_mc(mci)) {
+		ret = -ENODEV;
+		goto bail_mc;
+	}
+
+	return ret;
+
+bail_mc:
+	edac_mc_free(mci);
+
+bail_disable_ch1:
+	pci_disable_device(ch1mm);
+
+bail_ch1:
+	pci_dev_put(ch1mm);
+
+bail_disable_ch0:
+	pci_disable_device(ch0mm);
+
+bail_ch0:
+	pci_dev_put(ch0mm);
+
+bail_pdev:
+	pci_disable_device(pdev);
+
+bail:
+	return ret;
+}
+
+static void __devexit i5100_remove_one(struct pci_dev *pdev)
+{
+	struct mem_ctl_info *mci;
+	struct i5100_priv *priv;
+
+	mci = edac_mc_del_mc(&pdev->dev);
+
+	if (!mci)
+		return;
+
+	priv = mci->pvt_info;
+	pci_disable_device(pdev);
+	pci_disable_device(priv->ch0mm);
+	pci_disable_device(priv->ch1mm);
+	pci_dev_put(priv->ch0mm);
+	pci_dev_put(priv->ch1mm);
+
+	edac_mc_free(mci);
+}
+
+static const struct pci_device_id i5100_pci_tbl[] __devinitdata = {
+	/* Device 16, Function 0, Channel 0 Memory Map, Error Flag/Mask, ... */
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5100_16) },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, i5100_pci_tbl);
+
+static struct pci_driver i5100_driver = {
+	.name = KBUILD_BASENAME,
+	.probe = i5100_init_one,
+	.remove = __devexit_p(i5100_remove_one),
+	.id_table = i5100_pci_tbl,
+};
+
+static int __init i5100_init(void)
+{
+	int pci_rc;
+
+	pci_rc = pci_register_driver(&i5100_driver);
+
+	return (pci_rc < 0) ? pci_rc : 0;
+}
+
+static void __exit i5100_exit(void)
+{
+	pci_unregister_driver(&i5100_driver);
+}
+
+module_init(i5100_init);
+module_exit(i5100_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR
+    ("Arthur Jones <ajones@riverbed.com>");
+MODULE_DESCRIPTION("MC Driver for Intel I5100 memory controllers");
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index d49361bfe67..2265d9ca153 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -195,14 +195,15 @@ static irqreturn_t mpc85xx_pci_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int __devinit mpc85xx_pci_err_probe(struct platform_device *pdev)
+static int __devinit mpc85xx_pci_err_probe(struct of_device *op,
+					   const struct of_device_id *match)
 {
 	struct edac_pci_ctl_info *pci;
 	struct mpc85xx_pci_pdata *pdata;
-	struct resource *r;
+	struct resource r;
 	int res = 0;
 
-	if (!devres_open_group(&pdev->dev, mpc85xx_pci_err_probe, GFP_KERNEL))
+	if (!devres_open_group(&op->dev, mpc85xx_pci_err_probe, GFP_KERNEL))
 		return -ENOMEM;
 
 	pci = edac_pci_alloc_ctl_info(sizeof(*pdata), "mpc85xx_pci_err");
@@ -212,34 +213,37 @@ static int __devinit mpc85xx_pci_err_probe(struct platform_device *pdev)
 	pdata = pci->pvt_info;
 	pdata->name = "mpc85xx_pci_err";
 	pdata->irq = NO_IRQ;
-	platform_set_drvdata(pdev, pci);
-	pci->dev = &pdev->dev;
+	dev_set_drvdata(&op->dev, pci);
+	pci->dev = &op->dev;
 	pci->mod_name = EDAC_MOD_STR;
 	pci->ctl_name = pdata->name;
-	pci->dev_name = pdev->dev.bus_id;
+	pci->dev_name = op->dev.bus_id;
 
 	if (edac_op_state == EDAC_OPSTATE_POLL)
 		pci->edac_check = mpc85xx_pci_check;
 
 	pdata->edac_idx = edac_pci_idx++;
 
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!r) {
+	res = of_address_to_resource(op->node, 0, &r);
+	if (res) {
 		printk(KERN_ERR "%s: Unable to get resource for "
 		       "PCI err regs\n", __func__);
 		goto err;
 	}
 
-	if (!devm_request_mem_region(&pdev->dev, r->start,
-				     r->end - r->start + 1, pdata->name)) {
+	/* we only need the error registers */
+	r.start += 0xe00;
+
+	if (!devm_request_mem_region(&op->dev, r.start,
+					r.end - r.start + 1, pdata->name)) {
 		printk(KERN_ERR "%s: Error while requesting mem region\n",
 		       __func__);
 		res = -EBUSY;
 		goto err;
 	}
 
-	pdata->pci_vbase = devm_ioremap(&pdev->dev, r->start,
-					r->end - r->start + 1);
+	pdata->pci_vbase = devm_ioremap(&op->dev, r.start,
+					r.end - r.start + 1);
 	if (!pdata->pci_vbase) {
 		printk(KERN_ERR "%s: Unable to setup PCI err regs\n", __func__);
 		res = -ENOMEM;
@@ -266,14 +270,15 @@ static int __devinit mpc85xx_pci_err_probe(struct platform_device *pdev)
 	}
 
 	if (edac_op_state == EDAC_OPSTATE_INT) {
-		pdata->irq = platform_get_irq(pdev, 0);
-		res = devm_request_irq(&pdev->dev, pdata->irq,
+		pdata->irq = irq_of_parse_and_map(op->node, 0);
+		res = devm_request_irq(&op->dev, pdata->irq,
 				       mpc85xx_pci_isr, IRQF_DISABLED,
 				       "[EDAC] PCI err", pci);
 		if (res < 0) {
 			printk(KERN_ERR
 			       "%s: Unable to requiest irq %d for "
 			       "MPC85xx PCI err\n", __func__, pdata->irq);
+			irq_dispose_mapping(pdata->irq);
 			res = -ENODEV;
 			goto err2;
 		}
@@ -282,23 +287,23 @@ static int __devinit mpc85xx_pci_err_probe(struct platform_device *pdev)
 		       pdata->irq);
 	}
 
-	devres_remove_group(&pdev->dev, mpc85xx_pci_err_probe);
+	devres_remove_group(&op->dev, mpc85xx_pci_err_probe);
 	debugf3("%s(): success\n", __func__);
 	printk(KERN_INFO EDAC_MOD_STR " PCI err registered\n");
 
 	return 0;
 
 err2:
-	edac_pci_del_device(&pdev->dev);
+	edac_pci_del_device(&op->dev);
 err:
 	edac_pci_free_ctl_info(pci);
-	devres_release_group(&pdev->dev, mpc85xx_pci_err_probe);
+	devres_release_group(&op->dev, mpc85xx_pci_err_probe);
 	return res;
 }
 
-static int mpc85xx_pci_err_remove(struct platform_device *pdev)
+static int mpc85xx_pci_err_remove(struct of_device *op)
 {
-	struct edac_pci_ctl_info *pci = platform_get_drvdata(pdev);
+	struct edac_pci_ctl_info *pci = dev_get_drvdata(&op->dev);
 	struct mpc85xx_pci_pdata *pdata = pci->pvt_info;
 
 	debugf0("%s()\n", __func__);
@@ -318,12 +323,26 @@ static int mpc85xx_pci_err_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_driver mpc85xx_pci_err_driver = {
+static struct of_device_id mpc85xx_pci_err_of_match[] = {
+	{
+	 .compatible = "fsl,mpc8540-pcix",
+	 },
+	{
+	 .compatible = "fsl,mpc8540-pci",
+	},
+	{},
+};
+
+static struct of_platform_driver mpc85xx_pci_err_driver = {
+	.owner = THIS_MODULE,
+	.name = "mpc85xx_pci_err",
+	.match_table = mpc85xx_pci_err_of_match,
 	.probe = mpc85xx_pci_err_probe,
 	.remove = __devexit_p(mpc85xx_pci_err_remove),
 	.driver = {
-		.name = "mpc85xx_pci_err",
-	}
+		   .name = "mpc85xx_pci_err",
+		   .owner = THIS_MODULE,
+		   },
 };
 
 #endif				/* CONFIG_PCI */
@@ -1002,7 +1021,7 @@ static int __init mpc85xx_mc_init(void)
 		printk(KERN_WARNING EDAC_MOD_STR "L2 fails to register\n");
 
 #ifdef CONFIG_PCI
-	res = platform_driver_register(&mpc85xx_pci_err_driver);
+	res = of_register_platform_driver(&mpc85xx_pci_err_driver);
 	if (res)
 		printk(KERN_WARNING EDAC_MOD_STR "PCI fails to register\n");
 #endif
@@ -1025,7 +1044,7 @@ static void __exit mpc85xx_mc_exit(void)
 {
 	mtspr(SPRN_HID1, orig_hid1);
 #ifdef CONFIG_PCI
-	platform_driver_unregister(&mpc85xx_pci_err_driver);
+	of_unregister_platform_driver(&mpc85xx_pci_err_driver);
 #endif
 	of_unregister_platform_driver(&mpc85xx_l2_err_driver);
 	of_unregister_platform_driver(&mpc85xx_mc_err_driver);
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index bf071f140a0..083ce8d0c63 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -71,6 +71,35 @@ static irqreturn_t mv64x60_pci_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+/*
+ * Bit 0 of MV64x60_PCIx_ERR_MASK does not exist on the 64360 and because of
+ * errata FEr-#11 and FEr-##16 for the 64460, it should be 0 on that chip as
+ * well.  IOW, don't set bit 0.
+ */
+
+/* Erratum FEr PCI-#16: clear bit 0 of PCI SERRn Mask reg. */
+static int __init mv64x60_pci_fixup(struct platform_device *pdev)
+{
+	struct resource *r;
+	void __iomem *pci_serr;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!r) {
+		printk(KERN_ERR "%s: Unable to get resource for "
+		       "PCI err regs\n", __func__);
+		return -ENOENT;
+	}
+
+	pci_serr = ioremap(r->start, r->end - r->start + 1);
+	if (!pci_serr)
+		return -ENOMEM;
+
+	out_le32(pci_serr, in_le32(pci_serr) & ~0x1);
+	iounmap(pci_serr);
+
+	return 0;
+}
+
 static int __devinit mv64x60_pci_err_probe(struct platform_device *pdev)
 {
 	struct edac_pci_ctl_info *pci;
@@ -128,6 +157,12 @@ static int __devinit mv64x60_pci_err_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	res = mv64x60_pci_fixup(pdev);
+	if (res < 0) {
+		printk(KERN_ERR "%s: PCI fixup failed\n", __func__);
+		goto err;
+	}
+
 	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE, 0);
 	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_MASK, 0);
 	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_MASK,
@@ -612,7 +647,7 @@ static void get_total_mem(struct mv64x60_mc_pdata *pdata)
 	if (!np)
 		return;
 
-	reg = get_property(np, "reg", NULL);
+	reg = of_get_property(np, "reg", NULL);
 
 	pdata->total_mem = reg[1];
 }
diff --git a/drivers/firewire/fw-iso.c b/drivers/firewire/fw-iso.c
index bcbe794a3ea..e14c03dc006 100644
--- a/drivers/firewire/fw-iso.c
+++ b/drivers/firewire/fw-iso.c
@@ -50,7 +50,7 @@ fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
 
 		address = dma_map_page(card->device, buffer->pages[i],
 				       0, PAGE_SIZE, direction);
-		if (dma_mapping_error(address)) {
+		if (dma_mapping_error(card->device, address)) {
 			__free_page(buffer->pages[i]);
 			goto out_pages;
 		}
diff --git a/drivers/firewire/fw-ohci.c b/drivers/firewire/fw-ohci.c
index 333b12544dd..566672e0bcf 100644
--- a/drivers/firewire/fw-ohci.c
+++ b/drivers/firewire/fw-ohci.c
@@ -953,7 +953,7 @@ at_context_queue_packet(struct context *ctx, struct fw_packet *packet)
 		payload_bus =
 			dma_map_single(ohci->card.device, packet->payload,
 				       packet->payload_length, DMA_TO_DEVICE);
-		if (dma_mapping_error(payload_bus)) {
+		if (dma_mapping_error(ohci->card.device, payload_bus)) {
 			packet->ack = RCODE_SEND_ERROR;
 			return -1;
 		}
diff --git a/drivers/firewire/fw-sbp2.c b/drivers/firewire/fw-sbp2.c
index 53fc5a641e6..aaff50ebba1 100644
--- a/drivers/firewire/fw-sbp2.c
+++ b/drivers/firewire/fw-sbp2.c
@@ -543,7 +543,7 @@ sbp2_send_management_orb(struct sbp2_logical_unit *lu, int node_id,
 	orb->response_bus =
 		dma_map_single(device->card->device, &orb->response,
 			       sizeof(orb->response), DMA_FROM_DEVICE);
-	if (dma_mapping_error(orb->response_bus))
+	if (dma_mapping_error(device->card->device, orb->response_bus))
 		goto fail_mapping_response;
 
 	orb->request.response.high = 0;
@@ -577,7 +577,7 @@ sbp2_send_management_orb(struct sbp2_logical_unit *lu, int node_id,
 	orb->base.request_bus =
 		dma_map_single(device->card->device, &orb->request,
 			       sizeof(orb->request), DMA_TO_DEVICE);
-	if (dma_mapping_error(orb->base.request_bus))
+	if (dma_mapping_error(device->card->device, orb->base.request_bus))
 		goto fail_mapping_request;
 
 	sbp2_send_orb(&orb->base, lu, node_id, generation,
@@ -1424,7 +1424,7 @@ sbp2_map_scatterlist(struct sbp2_command_orb *orb, struct fw_device *device,
 	orb->page_table_bus =
 		dma_map_single(device->card->device, orb->page_table,
 			       sizeof(orb->page_table), DMA_TO_DEVICE);
-	if (dma_mapping_error(orb->page_table_bus))
+	if (dma_mapping_error(device->card->device, orb->page_table_bus))
 		goto fail_page_table;
 
 	/*
@@ -1509,7 +1509,7 @@ static int sbp2_scsi_queuecommand(struct scsi_cmnd *cmd, scsi_done_fn_t done)
 	orb->base.request_bus =
 		dma_map_single(device->card->device, &orb->request,
 			       sizeof(orb->request), DMA_TO_DEVICE);
-	if (dma_mapping_error(orb->base.request_bus))
+	if (dma_mapping_error(device->card->device, orb->base.request_bus))
 		goto out;
 
 	sbp2_send_orb(&orb->base, lu, lu->tgt->node_id, lu->generation,
diff --git a/drivers/firmware/dcdbas.c b/drivers/firmware/dcdbas.c
index 0b624e927a6..c66817e7717 100644
--- a/drivers/firmware/dcdbas.c
+++ b/drivers/firmware/dcdbas.c
@@ -152,20 +152,11 @@ static ssize_t smi_data_read(struct kobject *kobj,
 			     struct bin_attribute *bin_attr,
 			     char *buf, loff_t pos, size_t count)
 {
-	size_t max_read;
 	ssize_t ret;
 
 	mutex_lock(&smi_data_lock);
-
-	if (pos >= smi_data_buf_size) {
-		ret = 0;
-		goto out;
-	}
-
-	max_read = smi_data_buf_size - pos;
-	ret = min(max_read, count);
-	memcpy(buf, smi_data_buf + pos, ret);
-out:
+	ret = memory_read_from_buffer(buf, count, &pos, smi_data_buf,
+					smi_data_buf_size);
 	mutex_unlock(&smi_data_lock);
 	return ret;
 }
diff --git a/drivers/firmware/dell_rbu.c b/drivers/firmware/dell_rbu.c
index 7430e218cda..13946ebd77d 100644
--- a/drivers/firmware/dell_rbu.c
+++ b/drivers/firmware/dell_rbu.c
@@ -507,11 +507,6 @@ static ssize_t read_packet_data(char *buffer, loff_t pos, size_t count)
 
 static ssize_t read_rbu_mono_data(char *buffer, loff_t pos, size_t count)
 {
-	unsigned char *ptemp = NULL;
-	size_t bytes_left = 0;
-	size_t data_length = 0;
-	ssize_t ret_count = 0;
-
 	/* check to see if we have something to return */
 	if ((rbu_data.image_update_buffer == NULL) ||
 		(rbu_data.bios_image_size == 0)) {
@@ -519,28 +514,11 @@ static ssize_t read_rbu_mono_data(char *buffer, loff_t pos, size_t count)
 			"bios_image_size %lu\n",
 			rbu_data.image_update_buffer,
 			rbu_data.bios_image_size);
-		ret_count = -ENOMEM;
-		goto read_rbu_data_exit;
-	}
-
-	if (pos > rbu_data.bios_image_size) {
-		ret_count = 0;
-		goto read_rbu_data_exit;
+		return -ENOMEM;
 	}
 
-	bytes_left = rbu_data.bios_image_size - pos;
-	data_length = min(bytes_left, count);
-
-	ptemp = rbu_data.image_update_buffer;
-	memcpy(buffer, (ptemp + pos), data_length);
-
-	if ((pos + count) > rbu_data.bios_image_size)
-		/* this was the last copy */
-		ret_count = bytes_left;
-	else
-		ret_count = count;
-      read_rbu_data_exit:
-	return ret_count;
+	return memory_read_from_buffer(buffer, count, &pos,
+			rbu_data.image_update_buffer, rbu_data.bios_image_size);
 }
 
 static ssize_t read_rbu_data(struct kobject *kobj,
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index e23399c7f77..001622eb86f 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -153,12 +153,14 @@ int __init firmware_map_add_early(resource_size_t start, resource_size_t end,
 
 static ssize_t start_show(struct firmware_map_entry *entry, char *buf)
 {
-	return snprintf(buf, PAGE_SIZE, "0x%llx\n", entry->start);
+	return snprintf(buf, PAGE_SIZE, "0x%llx\n",
+		(unsigned long long)entry->start);
 }
 
 static ssize_t end_show(struct firmware_map_entry *entry, char *buf)
 {
-	return snprintf(buf, PAGE_SIZE, "0x%llx\n", entry->end);
+	return snprintf(buf, PAGE_SIZE, "0x%llx\n",
+		(unsigned long long)entry->end);
 }
 
 static ssize_t type_show(struct firmware_map_entry *entry, char *buf)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index fced1909cbb..dbd42d6c93a 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -2,15 +2,40 @@
 # GPIO infrastructure and expanders
 #
 
-config HAVE_GPIO_LIB
+config ARCH_WANT_OPTIONAL_GPIOLIB
 	bool
 	help
+	  Select this config option from the architecture Kconfig, if
+	  it is possible to use gpiolib on the architecture, but let the
+	  user decide whether to actually build it or not.
+	  Select this instead of ARCH_REQUIRE_GPIOLIB, if your architecture does
+	  not depend on GPIOs being available, but rather let the user
+	  decide whether he needs it or not.
+
+config ARCH_REQUIRE_GPIOLIB
+	bool
+	select GPIOLIB
+	help
 	  Platforms select gpiolib if they use this infrastructure
 	  for all their GPIOs, usually starting with ones integrated
 	  into SOC processors.
+	  Selecting this from the architecture code will cause the gpiolib
+	  code to always get built in.
+
+
+
+menuconfig GPIOLIB
+	bool "GPIO Support"
+	depends on ARCH_WANT_OPTIONAL_GPIOLIB || ARCH_REQUIRE_GPIOLIB
+	select GENERIC_GPIO
+	help
+	  This enables GPIO support through the generic GPIO library.
+	  You only need to enable this, if you also want to enable
+	  one or more of the GPIO expansion card drivers below.
 
-menu "GPIO Support"
-	depends on HAVE_GPIO_LIB
+	  If unsure, say N.
+
+if GPIOLIB
 
 config DEBUG_GPIO
 	bool "Debug GPIO calls"
@@ -23,10 +48,44 @@ config DEBUG_GPIO
 	  slower.  The diagnostics help catch the type of setup errors
 	  that are most common when setting up new platforms or boards.
 
+config GPIO_SYSFS
+	bool "/sys/class/gpio/... (sysfs interface)"
+	depends on SYSFS && EXPERIMENTAL
+	help
+	  Say Y here to add a sysfs interface for GPIOs.
+
+	  This is mostly useful to work around omissions in a system's
+	  kernel support.  Those are common in custom and semicustom
+	  hardware assembled using standard kernels with a minimum of
+	  custom patches.  In those cases, userspace code may import
+	  a given GPIO from the kernel, if no kernel driver requested it.
+
+	  Kernel drivers may also request that a particular GPIO be
+	  exported to userspace; this can be useful when debugging.
+
 # put expanders in the right section, in alphabetical order
 
 comment "I2C GPIO expanders:"
 
+config GPIO_MAX732X
+	tristate "MAX7319, MAX7320-7327 I2C Port Expanders"
+	depends on I2C
+	help
+	  Say yes here to support the MAX7319, MAX7320-7327 series of I2C
+	  Port Expanders. Each IO port on these chips has a fixed role of
+	  Input (designated by 'I'), Push-Pull Output ('O'), or Open-Drain
+	  Input and Output (designed by 'P'). The combinations are listed
+	  below:
+
+	  8 bits:	max7319 (8I), max7320 (8O), max7321 (8P),
+		  	max7322 (4I4O), max7323 (4P4O)
+
+	  16 bits:	max7324 (8I8O), max7325 (8P8O),
+		  	max7326 (4I12O), max7327 (4P12O)
+
+	  Board setup code must specify the model to use, and the start
+	  number for these GPIOs.
+
 config GPIO_PCA953X
 	tristate "PCA953x, PCA955x, and MAX7310 I/O ports"
 	depends on I2C
@@ -68,6 +127,24 @@ config GPIO_PCF857X
 	  This driver provides an in-kernel interface to those GPIOs using
 	  platform-neutral GPIO calls.
 
+comment "PCI GPIO expanders:"
+
+config GPIO_BT8XX
+	tristate "BT8XX GPIO abuser"
+	depends on PCI && VIDEO_BT848=n
+	help
+	  The BT8xx frame grabber chip has 24 GPIO pins than can be abused
+	  as a cheap PCI GPIO card.
+
+	  This chip can be found on Miro, Hauppauge and STB TV-cards.
+
+	  The card needs to be physically altered for using it as a
+	  GPIO card. For more information on how to build a GPIO card
+	  from a BT8xx TV card, see the documentation file at
+	  Documentation/bt8xxgpio.txt
+
+	  If unsure, say N.
+
 comment "SPI GPIO expanders:"
 
 config GPIO_MAX7301
@@ -83,4 +160,4 @@ config GPIO_MCP23S08
 	  SPI driver for Microchip MCP23S08 I/O expander.  This provides
 	  a GPIO interface supporting inputs and outputs.
 
-endmenu
+endif
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 16e796dc541..01b4bbde195 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -2,9 +2,11 @@
 
 ccflags-$(CONFIG_DEBUG_GPIO)	+= -DDEBUG
 
-obj-$(CONFIG_HAVE_GPIO_LIB)	+= gpiolib.o
+obj-$(CONFIG_GPIOLIB)		+= gpiolib.o
 
 obj-$(CONFIG_GPIO_MAX7301)	+= max7301.o
+obj-$(CONFIG_GPIO_MAX732X)	+= max732x.o
 obj-$(CONFIG_GPIO_MCP23S08)	+= mcp23s08.o
 obj-$(CONFIG_GPIO_PCA953X)	+= pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= pcf857x.o
+obj-$(CONFIG_GPIO_BT8XX)	+= bt8xxgpio.o
diff --git a/drivers/gpio/bt8xxgpio.c b/drivers/gpio/bt8xxgpio.c
new file mode 100644
index 00000000000..7a1168249dd
--- /dev/null
+++ b/drivers/gpio/bt8xxgpio.c
@@ -0,0 +1,348 @@
+/*
+
+    bt8xx GPIO abuser
+
+    Copyright (C) 2008 Michael Buesch <mb@bu3sch.de>
+
+    Please do _only_ contact the people listed _above_ with issues related to this driver.
+    All the other people listed below are not related to this driver. Their names
+    are only here, because this driver is derived from the bt848 driver.
+
+
+    Derived from the bt848 driver:
+
+    Copyright (C) 1996,97,98 Ralph  Metzler
+			   & Marcus Metzler
+    (c) 1999-2002 Gerd Knorr
+
+    some v4l2 code lines are taken from Justin's bttv2 driver which is
+    (c) 2000 Justin Schoeman
+
+    V4L1 removal from:
+    (c) 2005-2006 Nickolay V. Shmyrev
+
+    Fixes to be fully V4L2 compliant by
+    (c) 2006 Mauro Carvalho Chehab
+
+    Cropping and overscan support
+    Copyright (C) 2005, 2006 Michael H. Schimek
+    Sponsored by OPQ Systems AB
+
+    This program is free software; 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.
+*/
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include <asm/gpio.h>
+
+/* Steal the hardware definitions from the bttv driver. */
+#include "../media/video/bt8xx/bt848.h"
+
+
+#define BT8XXGPIO_NR_GPIOS		24 /* We have 24 GPIO pins */
+
+
+struct bt8xxgpio {
+	spinlock_t lock;
+
+	void __iomem *mmio;
+	struct pci_dev *pdev;
+	struct gpio_chip gpio;
+
+#ifdef CONFIG_PM
+	u32 saved_outen;
+	u32 saved_data;
+#endif
+};
+
+#define bgwrite(dat, adr)	writel((dat), bg->mmio+(adr))
+#define bgread(adr)		readl(bg->mmio+(adr))
+
+
+static int modparam_gpiobase = -1/* dynamic */;
+module_param_named(gpiobase, modparam_gpiobase, int, 0444);
+MODULE_PARM_DESC(gpiobase, "The GPIO number base. -1 means dynamic, which is the default.");
+
+
+static int bt8xxgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
+{
+	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	unsigned long flags;
+	u32 outen, data;
+
+	spin_lock_irqsave(&bg->lock, flags);
+
+	data = bgread(BT848_GPIO_DATA);
+	data &= ~(1 << nr);
+	bgwrite(data, BT848_GPIO_DATA);
+
+	outen = bgread(BT848_GPIO_OUT_EN);
+	outen &= ~(1 << nr);
+	bgwrite(outen, BT848_GPIO_OUT_EN);
+
+	spin_unlock_irqrestore(&bg->lock, flags);
+
+	return 0;
+}
+
+static int bt8xxgpio_gpio_get(struct gpio_chip *gpio, unsigned nr)
+{
+	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&bg->lock, flags);
+	val = bgread(BT848_GPIO_DATA);
+	spin_unlock_irqrestore(&bg->lock, flags);
+
+	return !!(val & (1 << nr));
+}
+
+static int bt8xxgpio_gpio_direction_output(struct gpio_chip *gpio,
+					unsigned nr, int val)
+{
+	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	unsigned long flags;
+	u32 outen, data;
+
+	spin_lock_irqsave(&bg->lock, flags);
+
+	outen = bgread(BT848_GPIO_OUT_EN);
+	outen |= (1 << nr);
+	bgwrite(outen, BT848_GPIO_OUT_EN);
+
+	data = bgread(BT848_GPIO_DATA);
+	if (val)
+		data |= (1 << nr);
+	else
+		data &= ~(1 << nr);
+	bgwrite(data, BT848_GPIO_DATA);
+
+	spin_unlock_irqrestore(&bg->lock, flags);
+
+	return 0;
+}
+
+static void bt8xxgpio_gpio_set(struct gpio_chip *gpio,
+			    unsigned nr, int val)
+{
+	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	unsigned long flags;
+	u32 data;
+
+	spin_lock_irqsave(&bg->lock, flags);
+
+	data = bgread(BT848_GPIO_DATA);
+	if (val)
+		data |= (1 << nr);
+	else
+		data &= ~(1 << nr);
+	bgwrite(data, BT848_GPIO_DATA);
+
+	spin_unlock_irqrestore(&bg->lock, flags);
+}
+
+static void bt8xxgpio_gpio_setup(struct bt8xxgpio *bg)
+{
+	struct gpio_chip *c = &bg->gpio;
+
+	c->label = bg->pdev->dev.bus_id;
+	c->owner = THIS_MODULE;
+	c->direction_input = bt8xxgpio_gpio_direction_input;
+	c->get = bt8xxgpio_gpio_get;
+	c->direction_output = bt8xxgpio_gpio_direction_output;
+	c->set = bt8xxgpio_gpio_set;
+	c->dbg_show = NULL;
+	c->base = modparam_gpiobase;
+	c->ngpio = BT8XXGPIO_NR_GPIOS;
+	c->can_sleep = 0;
+}
+
+static int bt8xxgpio_probe(struct pci_dev *dev,
+			const struct pci_device_id *pci_id)
+{
+	struct bt8xxgpio *bg;
+	int err;
+
+	bg = kzalloc(sizeof(*bg), GFP_KERNEL);
+	if (!bg)
+		return -ENOMEM;
+
+	bg->pdev = dev;
+	spin_lock_init(&bg->lock);
+
+	err = pci_enable_device(dev);
+	if (err) {
+		printk(KERN_ERR "bt8xxgpio: Can't enable device.\n");
+		goto err_freebg;
+	}
+	if (!request_mem_region(pci_resource_start(dev, 0),
+				pci_resource_len(dev, 0),
+				"bt8xxgpio")) {
+		printk(KERN_WARNING "bt8xxgpio: Can't request iomem (0x%llx).\n",
+		       (unsigned long long)pci_resource_start(dev, 0));
+		err = -EBUSY;
+		goto err_disable;
+	}
+	pci_set_master(dev);
+	pci_set_drvdata(dev, bg);
+
+	bg->mmio = ioremap(pci_resource_start(dev, 0), 0x1000);
+	if (!bg->mmio) {
+		printk(KERN_ERR "bt8xxgpio: ioremap() failed\n");
+		err = -EIO;
+		goto err_release_mem;
+	}
+
+	/* Disable interrupts */
+	bgwrite(0, BT848_INT_MASK);
+
+	/* gpio init */
+	bgwrite(0, BT848_GPIO_DMA_CTL);
+	bgwrite(0, BT848_GPIO_REG_INP);
+	bgwrite(0, BT848_GPIO_OUT_EN);
+
+	bt8xxgpio_gpio_setup(bg);
+	err = gpiochip_add(&bg->gpio);
+	if (err) {
+		printk(KERN_ERR "bt8xxgpio: Failed to register GPIOs\n");
+		goto err_release_mem;
+	}
+
+	printk(KERN_INFO "bt8xxgpio: Abusing BT8xx card for GPIOs %d to %d\n",
+	       bg->gpio.base, bg->gpio.base + BT8XXGPIO_NR_GPIOS - 1);
+
+	return 0;
+
+err_release_mem:
+	release_mem_region(pci_resource_start(dev, 0),
+			   pci_resource_len(dev, 0));
+	pci_set_drvdata(dev, NULL);
+err_disable:
+	pci_disable_device(dev);
+err_freebg:
+	kfree(bg);
+
+	return err;
+}
+
+static void bt8xxgpio_remove(struct pci_dev *pdev)
+{
+	struct bt8xxgpio *bg = pci_get_drvdata(pdev);
+
+	gpiochip_remove(&bg->gpio);
+
+	bgwrite(0, BT848_INT_MASK);
+	bgwrite(~0x0, BT848_INT_STAT);
+	bgwrite(0x0, BT848_GPIO_OUT_EN);
+
+	iounmap(bg->mmio);
+	release_mem_region(pci_resource_start(pdev, 0),
+			   pci_resource_len(pdev, 0));
+	pci_disable_device(pdev);
+
+	pci_set_drvdata(pdev, NULL);
+	kfree(bg);
+}
+
+#ifdef CONFIG_PM
+static int bt8xxgpio_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct bt8xxgpio *bg = pci_get_drvdata(pdev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bg->lock, flags);
+
+	bg->saved_outen = bgread(BT848_GPIO_OUT_EN);
+	bg->saved_data = bgread(BT848_GPIO_DATA);
+
+	bgwrite(0, BT848_INT_MASK);
+	bgwrite(~0x0, BT848_INT_STAT);
+	bgwrite(0x0, BT848_GPIO_OUT_EN);
+
+	spin_unlock_irqrestore(&bg->lock, flags);
+
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+
+	return 0;
+}
+
+static int bt8xxgpio_resume(struct pci_dev *pdev)
+{
+	struct bt8xxgpio *bg = pci_get_drvdata(pdev);
+	unsigned long flags;
+	int err;
+
+	pci_set_power_state(pdev, 0);
+	err = pci_enable_device(pdev);
+	if (err)
+		return err;
+	pci_restore_state(pdev);
+
+	spin_lock_irqsave(&bg->lock, flags);
+
+	bgwrite(0, BT848_INT_MASK);
+	bgwrite(0, BT848_GPIO_DMA_CTL);
+	bgwrite(0, BT848_GPIO_REG_INP);
+	bgwrite(bg->saved_outen, BT848_GPIO_OUT_EN);
+	bgwrite(bg->saved_data & bg->saved_outen,
+		BT848_GPIO_DATA);
+
+	spin_unlock_irqrestore(&bg->lock, flags);
+
+	return 0;
+}
+#else
+#define bt8xxgpio_suspend NULL
+#define bt8xxgpio_resume NULL
+#endif /* CONFIG_PM */
+
+static struct pci_device_id bt8xxgpio_pci_tbl[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROOKTREE, PCI_DEVICE_ID_BT848) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROOKTREE, PCI_DEVICE_ID_BT849) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROOKTREE, PCI_DEVICE_ID_BT878) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROOKTREE, PCI_DEVICE_ID_BT879) },
+	{ 0, },
+};
+MODULE_DEVICE_TABLE(pci, bt8xxgpio_pci_tbl);
+
+static struct pci_driver bt8xxgpio_pci_driver = {
+	.name		= "bt8xxgpio",
+	.id_table	= bt8xxgpio_pci_tbl,
+	.probe		= bt8xxgpio_probe,
+	.remove		= bt8xxgpio_remove,
+	.suspend	= bt8xxgpio_suspend,
+	.resume		= bt8xxgpio_resume,
+};
+
+static int bt8xxgpio_init(void)
+{
+	return pci_register_driver(&bt8xxgpio_pci_driver);
+}
+module_init(bt8xxgpio_init)
+
+static void bt8xxgpio_exit(void)
+{
+	pci_unregister_driver(&bt8xxgpio_pci_driver);
+}
+module_exit(bt8xxgpio_exit)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Buesch");
+MODULE_DESCRIPTION("Abuse a BT8xx framegrabber card as generic GPIO card");
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index beaf6b3a37d..8d2940517c9 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -2,8 +2,11 @@
 #include <linux/module.h>
 #include <linux/irq.h>
 #include <linux/spinlock.h>
-
-#include <asm/gpio.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/gpio.h>
 
 
 /* Optional implementation infrastructure for GPIO interfaces.
@@ -44,6 +47,8 @@ struct gpio_desc {
 #define FLAG_REQUESTED	0
 #define FLAG_IS_OUT	1
 #define FLAG_RESERVED	2
+#define FLAG_EXPORT	3	/* protected by sysfs_lock */
+#define FLAG_SYSFS	4	/* exported via /sys/class/gpio/control */
 
 #ifdef CONFIG_DEBUG_FS
 	const char		*label;
@@ -151,6 +156,482 @@ err:
 	return ret;
 }
 
+#ifdef CONFIG_GPIO_SYSFS
+
+/* lock protects against unexport_gpio() being called while
+ * sysfs files are active.
+ */
+static DEFINE_MUTEX(sysfs_lock);
+
+/*
+ * /sys/class/gpio/gpioN... only for GPIOs that are exported
+ *   /direction
+ *      * MAY BE OMITTED if kernel won't allow direction changes
+ *      * is read/write as "in" or "out"
+ *      * may also be written as "high" or "low", initializing
+ *        output value as specified ("out" implies "low")
+ *   /value
+ *      * always readable, subject to hardware behavior
+ *      * may be writable, as zero/nonzero
+ *
+ * REVISIT there will likely be an attribute for configuring async
+ * notifications, e.g. to specify polling interval or IRQ trigger type
+ * that would for example trigger a poll() on the "value".
+ */
+
+static ssize_t gpio_direction_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else
+		status = sprintf(buf, "%s\n",
+			test_bit(FLAG_IS_OUT, &desc->flags)
+				? "out" : "in");
+
+	mutex_unlock(&sysfs_lock);
+	return status;
+}
+
+static ssize_t gpio_direction_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	unsigned		gpio = desc - gpio_desc;
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else if (sysfs_streq(buf, "high"))
+		status = gpio_direction_output(gpio, 1);
+	else if (sysfs_streq(buf, "out") || sysfs_streq(buf, "low"))
+		status = gpio_direction_output(gpio, 0);
+	else if (sysfs_streq(buf, "in"))
+		status = gpio_direction_input(gpio);
+	else
+		status = -EINVAL;
+
+	mutex_unlock(&sysfs_lock);
+	return status ? : size;
+}
+
+static const DEVICE_ATTR(direction, 0644,
+		gpio_direction_show, gpio_direction_store);
+
+static ssize_t gpio_value_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	unsigned		gpio = desc - gpio_desc;
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else
+		status = sprintf(buf, "%d\n", gpio_get_value_cansleep(gpio));
+
+	mutex_unlock(&sysfs_lock);
+	return status;
+}
+
+static ssize_t gpio_value_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	unsigned		gpio = desc - gpio_desc;
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else if (!test_bit(FLAG_IS_OUT, &desc->flags))
+		status = -EPERM;
+	else {
+		long		value;
+
+		status = strict_strtol(buf, 0, &value);
+		if (status == 0) {
+			gpio_set_value_cansleep(gpio, value != 0);
+			status = size;
+		}
+	}
+
+	mutex_unlock(&sysfs_lock);
+	return status;
+}
+
+static /*const*/ DEVICE_ATTR(value, 0644,
+		gpio_value_show, gpio_value_store);
+
+static const struct attribute *gpio_attrs[] = {
+	&dev_attr_direction.attr,
+	&dev_attr_value.attr,
+	NULL,
+};
+
+static const struct attribute_group gpio_attr_group = {
+	.attrs = (struct attribute **) gpio_attrs,
+};
+
+/*
+ * /sys/class/gpio/gpiochipN/
+ *   /base ... matching gpio_chip.base (N)
+ *   /label ... matching gpio_chip.label
+ *   /ngpio ... matching gpio_chip.ngpio
+ */
+
+static ssize_t chip_base_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	const struct gpio_chip	*chip = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", chip->base);
+}
+static DEVICE_ATTR(base, 0444, chip_base_show, NULL);
+
+static ssize_t chip_label_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	const struct gpio_chip	*chip = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", chip->label ? : "");
+}
+static DEVICE_ATTR(label, 0444, chip_label_show, NULL);
+
+static ssize_t chip_ngpio_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	const struct gpio_chip	*chip = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%u\n", chip->ngpio);
+}
+static DEVICE_ATTR(ngpio, 0444, chip_ngpio_show, NULL);
+
+static const struct attribute *gpiochip_attrs[] = {
+	&dev_attr_base.attr,
+	&dev_attr_label.attr,
+	&dev_attr_ngpio.attr,
+	NULL,
+};
+
+static const struct attribute_group gpiochip_attr_group = {
+	.attrs = (struct attribute **) gpiochip_attrs,
+};
+
+/*
+ * /sys/class/gpio/export ... write-only
+ *	integer N ... number of GPIO to export (full access)
+ * /sys/class/gpio/unexport ... write-only
+ *	integer N ... number of GPIO to unexport
+ */
+static ssize_t export_store(struct class *class, const char *buf, size_t len)
+{
+	long	gpio;
+	int	status;
+
+	status = strict_strtol(buf, 0, &gpio);
+	if (status < 0)
+		goto done;
+
+	/* No extra locking here; FLAG_SYSFS just signifies that the
+	 * request and export were done by on behalf of userspace, so
+	 * they may be undone on its behalf too.
+	 */
+
+	status = gpio_request(gpio, "sysfs");
+	if (status < 0)
+		goto done;
+
+	status = gpio_export(gpio, true);
+	if (status < 0)
+		gpio_free(gpio);
+	else
+		set_bit(FLAG_SYSFS, &gpio_desc[gpio].flags);
+
+done:
+	if (status)
+		pr_debug("%s: status %d\n", __func__, status);
+	return status ? : len;
+}
+
+static ssize_t unexport_store(struct class *class, const char *buf, size_t len)
+{
+	long	gpio;
+	int	status;
+
+	status = strict_strtol(buf, 0, &gpio);
+	if (status < 0)
+		goto done;
+
+	status = -EINVAL;
+
+	/* reject bogus commands (gpio_unexport ignores them) */
+	if (!gpio_is_valid(gpio))
+		goto done;
+
+	/* No extra locking here; FLAG_SYSFS just signifies that the
+	 * request and export were done by on behalf of userspace, so
+	 * they may be undone on its behalf too.
+	 */
+	if (test_and_clear_bit(FLAG_SYSFS, &gpio_desc[gpio].flags)) {
+		status = 0;
+		gpio_free(gpio);
+	}
+done:
+	if (status)
+		pr_debug("%s: status %d\n", __func__, status);
+	return status ? : len;
+}
+
+static struct class_attribute gpio_class_attrs[] = {
+	__ATTR(export, 0200, NULL, export_store),
+	__ATTR(unexport, 0200, NULL, unexport_store),
+	__ATTR_NULL,
+};
+
+static struct class gpio_class = {
+	.name =		"gpio",
+	.owner =	THIS_MODULE,
+
+	.class_attrs =	gpio_class_attrs,
+};
+
+
+/**
+ * gpio_export - export a GPIO through sysfs
+ * @gpio: gpio to make available, already requested
+ * @direction_may_change: true if userspace may change gpio direction
+ * Context: arch_initcall or later
+ *
+ * When drivers want to make a GPIO accessible to userspace after they
+ * have requested it -- perhaps while debugging, or as part of their
+ * public interface -- they may use this routine.  If the GPIO can
+ * change direction (some can't) and the caller allows it, userspace
+ * will see "direction" sysfs attribute which may be used to change
+ * the gpio's direction.  A "value" attribute will always be provided.
+ *
+ * Returns zero on success, else an error.
+ */
+int gpio_export(unsigned gpio, bool direction_may_change)
+{
+	unsigned long		flags;
+	struct gpio_desc	*desc;
+	int			status = -EINVAL;
+
+	/* can't export until sysfs is available ... */
+	if (!gpio_class.p) {
+		pr_debug("%s: called too early!\n", __func__);
+		return -ENOENT;
+	}
+
+	if (!gpio_is_valid(gpio))
+		goto done;
+
+	mutex_lock(&sysfs_lock);
+
+	spin_lock_irqsave(&gpio_lock, flags);
+	desc = &gpio_desc[gpio];
+	if (test_bit(FLAG_REQUESTED, &desc->flags)
+			&& !test_bit(FLAG_EXPORT, &desc->flags)) {
+		status = 0;
+		if (!desc->chip->direction_input
+				|| !desc->chip->direction_output)
+			direction_may_change = false;
+	}
+	spin_unlock_irqrestore(&gpio_lock, flags);
+
+	if (status == 0) {
+		struct device	*dev;
+
+		dev = device_create(&gpio_class, desc->chip->dev, MKDEV(0, 0),
+					desc, "gpio%d", gpio);
+		if (dev) {
+			if (direction_may_change)
+				status = sysfs_create_group(&dev->kobj,
+						&gpio_attr_group);
+			else
+				status = device_create_file(dev,
+						&dev_attr_value);
+			if (status != 0)
+				device_unregister(dev);
+		} else
+			status = -ENODEV;
+		if (status == 0)
+			set_bit(FLAG_EXPORT, &desc->flags);
+	}
+
+	mutex_unlock(&sysfs_lock);
+
+done:
+	if (status)
+		pr_debug("%s: gpio%d status %d\n", __func__, gpio, status);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(gpio_export);
+
+static int match_export(struct device *dev, void *data)
+{
+	return dev_get_drvdata(dev) == data;
+}
+
+/**
+ * gpio_unexport - reverse effect of gpio_export()
+ * @gpio: gpio to make unavailable
+ *
+ * This is implicit on gpio_free().
+ */
+void gpio_unexport(unsigned gpio)
+{
+	struct gpio_desc	*desc;
+	int			status = -EINVAL;
+
+	if (!gpio_is_valid(gpio))
+		goto done;
+
+	mutex_lock(&sysfs_lock);
+
+	desc = &gpio_desc[gpio];
+	if (test_bit(FLAG_EXPORT, &desc->flags)) {
+		struct device	*dev = NULL;
+
+		dev = class_find_device(&gpio_class, NULL, desc, match_export);
+		if (dev) {
+			clear_bit(FLAG_EXPORT, &desc->flags);
+			put_device(dev);
+			device_unregister(dev);
+			status = 0;
+		} else
+			status = -ENODEV;
+	}
+
+	mutex_unlock(&sysfs_lock);
+done:
+	if (status)
+		pr_debug("%s: gpio%d status %d\n", __func__, gpio, status);
+}
+EXPORT_SYMBOL_GPL(gpio_unexport);
+
+static int gpiochip_export(struct gpio_chip *chip)
+{
+	int		status;
+	struct device	*dev;
+
+	/* Many systems register gpio chips for SOC support very early,
+	 * before driver model support is available.  In those cases we
+	 * export this later, in gpiolib_sysfs_init() ... here we just
+	 * verify that _some_ field of gpio_class got initialized.
+	 */
+	if (!gpio_class.p)
+		return 0;
+
+	/* use chip->base for the ID; it's already known to be unique */
+	mutex_lock(&sysfs_lock);
+	dev = device_create(&gpio_class, chip->dev, MKDEV(0, 0), chip,
+				"gpiochip%d", chip->base);
+	if (dev) {
+		status = sysfs_create_group(&dev->kobj,
+				&gpiochip_attr_group);
+	} else
+		status = -ENODEV;
+	chip->exported = (status == 0);
+	mutex_unlock(&sysfs_lock);
+
+	if (status) {
+		unsigned long	flags;
+		unsigned	gpio;
+
+		spin_lock_irqsave(&gpio_lock, flags);
+		gpio = chip->base;
+		while (gpio_desc[gpio].chip == chip)
+			gpio_desc[gpio++].chip = NULL;
+		spin_unlock_irqrestore(&gpio_lock, flags);
+
+		pr_debug("%s: chip %s status %d\n", __func__,
+				chip->label, status);
+	}
+
+	return status;
+}
+
+static void gpiochip_unexport(struct gpio_chip *chip)
+{
+	int			status;
+	struct device		*dev;
+
+	mutex_lock(&sysfs_lock);
+	dev = class_find_device(&gpio_class, NULL, chip, match_export);
+	if (dev) {
+		put_device(dev);
+		device_unregister(dev);
+		chip->exported = 0;
+		status = 0;
+	} else
+		status = -ENODEV;
+	mutex_unlock(&sysfs_lock);
+
+	if (status)
+		pr_debug("%s: chip %s status %d\n", __func__,
+				chip->label, status);
+}
+
+static int __init gpiolib_sysfs_init(void)
+{
+	int		status;
+	unsigned long	flags;
+	unsigned	gpio;
+
+	status = class_register(&gpio_class);
+	if (status < 0)
+		return status;
+
+	/* Scan and register the gpio_chips which registered very
+	 * early (e.g. before the class_register above was called).
+	 *
+	 * We run before arch_initcall() so chip->dev nodes can have
+	 * registered, and so arch_initcall() can always gpio_export().
+	 */
+	spin_lock_irqsave(&gpio_lock, flags);
+	for (gpio = 0; gpio < ARCH_NR_GPIOS; gpio++) {
+		struct gpio_chip	*chip;
+
+		chip = gpio_desc[gpio].chip;
+		if (!chip || chip->exported)
+			continue;
+
+		spin_unlock_irqrestore(&gpio_lock, flags);
+		status = gpiochip_export(chip);
+		spin_lock_irqsave(&gpio_lock, flags);
+	}
+	spin_unlock_irqrestore(&gpio_lock, flags);
+
+
+	return status;
+}
+postcore_initcall(gpiolib_sysfs_init);
+
+#else
+static inline int gpiochip_export(struct gpio_chip *chip)
+{
+	return 0;
+}
+
+static inline void gpiochip_unexport(struct gpio_chip *chip)
+{
+}
+
+#endif /* CONFIG_GPIO_SYSFS */
+
 /**
  * gpiochip_add() - register a gpio_chip
  * @chip: the chip to register, with chip->base initialized
@@ -160,6 +641,11 @@ err:
  * because the chip->base is invalid or already associated with a
  * different chip.  Otherwise it returns zero as a success code.
  *
+ * When gpiochip_add() is called very early during boot, so that GPIOs
+ * can be freely used, the chip->dev device must be registered before
+ * the gpio framework's arch_initcall().  Otherwise sysfs initialization
+ * for GPIOs will fail rudely.
+ *
  * If chip->base is negative, this requests dynamic assignment of
  * a range of valid GPIOs.
  */
@@ -182,7 +668,7 @@ int gpiochip_add(struct gpio_chip *chip)
 		base = gpiochip_find_base(chip->ngpio);
 		if (base < 0) {
 			status = base;
-			goto fail_unlock;
+			goto unlock;
 		}
 		chip->base = base;
 	}
@@ -197,12 +683,23 @@ int gpiochip_add(struct gpio_chip *chip)
 	if (status == 0) {
 		for (id = base; id < base + chip->ngpio; id++) {
 			gpio_desc[id].chip = chip;
-			gpio_desc[id].flags = 0;
+
+			/* REVISIT:  most hardware initializes GPIOs as
+			 * inputs (often with pullups enabled) so power
+			 * usage is minimized.  Linux code should set the
+			 * gpio direction first thing; but until it does,
+			 * we may expose the wrong direction in sysfs.
+			 */
+			gpio_desc[id].flags = !chip->direction_input
+				? (1 << FLAG_IS_OUT)
+				: 0;
 		}
 	}
 
-fail_unlock:
+unlock:
 	spin_unlock_irqrestore(&gpio_lock, flags);
+	if (status == 0)
+		status = gpiochip_export(chip);
 fail:
 	/* failures here can mean systems won't boot... */
 	if (status)
@@ -239,6 +736,10 @@ int gpiochip_remove(struct gpio_chip *chip)
 	}
 
 	spin_unlock_irqrestore(&gpio_lock, flags);
+
+	if (status == 0)
+		gpiochip_unexport(chip);
+
 	return status;
 }
 EXPORT_SYMBOL_GPL(gpiochip_remove);
@@ -296,6 +797,8 @@ void gpio_free(unsigned gpio)
 		return;
 	}
 
+	gpio_unexport(gpio);
+
 	spin_lock_irqsave(&gpio_lock, flags);
 
 	desc = &gpio_desc[gpio];
@@ -534,10 +1037,6 @@ EXPORT_SYMBOL_GPL(gpio_set_value_cansleep);
 
 #ifdef CONFIG_DEBUG_FS
 
-#include <linux/debugfs.h>
-#include <linux/seq_file.h>
-
-
 static void gpiolib_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
 	unsigned		i;
@@ -614,17 +1113,28 @@ static int gpiolib_show(struct seq_file *s, void *unused)
 	/* REVISIT this isn't locked against gpio_chip removal ... */
 
 	for (gpio = 0; gpio_is_valid(gpio); gpio++) {
+		struct device *dev;
+
 		if (chip == gpio_desc[gpio].chip)
 			continue;
 		chip = gpio_desc[gpio].chip;
 		if (!chip)
 			continue;
 
-		seq_printf(s, "%sGPIOs %d-%d, %s%s:\n",
+		seq_printf(s, "%sGPIOs %d-%d",
 				started ? "\n" : "",
-				chip->base, chip->base + chip->ngpio - 1,
-				chip->label ? : "generic",
-				chip->can_sleep ? ", can sleep" : "");
+				chip->base, chip->base + chip->ngpio - 1);
+		dev = chip->dev;
+		if (dev)
+			seq_printf(s, ", %s/%s",
+				dev->bus ? dev->bus->name : "no-bus",
+				dev->bus_id);
+		if (chip->label)
+			seq_printf(s, ", %s", chip->label);
+		if (chip->can_sleep)
+			seq_printf(s, ", can sleep");
+		seq_printf(s, ":\n");
+
 		started = 1;
 		if (chip->dbg_show)
 			chip->dbg_show(s, chip);
diff --git a/drivers/gpio/max732x.c b/drivers/gpio/max732x.c
new file mode 100644
index 00000000000..b51c8135ca2
--- /dev/null
+++ b/drivers/gpio/max732x.c
@@ -0,0 +1,385 @@
+/*
+ *  max732x.c - I2C Port Expander with 8/16 I/O
+ *
+ *  Copyright (C) 2007 Marvell International Ltd.
+ *  Copyright (C) 2008 Jack Ren <jack.ren@marvell.com>
+ *  Copyright (C) 2008 Eric Miao <eric.miao@marvell.com>
+ *
+ *  Derived from drivers/gpio/pca953x.c
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/gpio.h>
+
+#include <linux/i2c.h>
+#include <linux/i2c/max732x.h>
+
+
+/*
+ * Each port of MAX732x (including MAX7319) falls into one of the
+ * following three types:
+ *
+ *   - Push Pull Output
+ *   - Input
+ *   - Open Drain I/O
+ *
+ * designated by 'O', 'I' and 'P' individually according to MAXIM's
+ * datasheets.
+ *
+ * There are two groups of I/O ports, each group usually includes
+ * up to 8 I/O ports, and is accessed by a specific I2C address:
+ *
+ *   - Group A : by I2C address 0b'110xxxx
+ *   - Group B : by I2C address 0b'101xxxx
+ *
+ * where 'xxxx' is decided by the connections of pin AD2/AD0.  The
+ * address used also affects the initial state of output signals.
+ *
+ * Within each group of ports, there are five known combinations of
+ * I/O ports: 4I4O, 4P4O, 8I, 8P, 8O, see the definitions below for
+ * the detailed organization of these ports.
+ *
+ * GPIO numbers start from 'gpio_base + 0' to 'gpio_base + 8/16',
+ * and GPIOs from GROUP_A are numbered before those from GROUP_B
+ * (if there are two groups).
+ *
+ * NOTE: MAX7328/MAX7329 are drop-in replacements for PCF8574/a, so
+ * they are not supported by this driver.
+ */
+
+#define PORT_NONE	0x0	/* '/' No Port */
+#define PORT_OUTPUT	0x1	/* 'O' Push-Pull, Output Only */
+#define PORT_INPUT	0x2	/* 'I' Input Only */
+#define PORT_OPENDRAIN	0x3	/* 'P' Open-Drain, I/O */
+
+#define IO_4I4O		0x5AA5	/* O7 O6 I5 I4 I3 I2 O1 O0 */
+#define IO_4P4O		0x5FF5	/* O7 O6 P5 P4 P3 P2 O1 O0 */
+#define IO_8I		0xAAAA	/* I7 I6 I5 I4 I3 I2 I1 I0 */
+#define IO_8P		0xFFFF	/* P7 P6 P5 P4 P3 P2 P1 P0 */
+#define IO_8O		0x5555	/* O7 O6 O5 O4 O3 O2 O1 O0 */
+
+#define GROUP_A(x)	((x) & 0xffff)	/* I2C Addr: 0b'110xxxx */
+#define GROUP_B(x)	((x) << 16)	/* I2C Addr: 0b'101xxxx */
+
+static const struct i2c_device_id max732x_id[] = {
+	{ "max7319", GROUP_A(IO_8I) },
+	{ "max7320", GROUP_B(IO_8O) },
+	{ "max7321", GROUP_A(IO_8P) },
+	{ "max7322", GROUP_A(IO_4I4O) },
+	{ "max7323", GROUP_A(IO_4P4O) },
+	{ "max7324", GROUP_A(IO_8I) | GROUP_B(IO_8O) },
+	{ "max7325", GROUP_A(IO_8P) | GROUP_B(IO_8O) },
+	{ "max7326", GROUP_A(IO_4I4O) | GROUP_B(IO_8O) },
+	{ "max7327", GROUP_A(IO_4P4O) | GROUP_B(IO_8O) },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, max732x_id);
+
+struct max732x_chip {
+	struct gpio_chip gpio_chip;
+
+	struct i2c_client *client;	/* "main" client */
+	struct i2c_client *client_dummy;
+	struct i2c_client *client_group_a;
+	struct i2c_client *client_group_b;
+
+	unsigned int	mask_group_a;
+	unsigned int	dir_input;
+	unsigned int	dir_output;
+
+	struct mutex	lock;
+	uint8_t		reg_out[2];
+};
+
+static int max732x_write(struct max732x_chip *chip, int group_a, uint8_t val)
+{
+	struct i2c_client *client;
+	int ret;
+
+	client = group_a ? chip->client_group_a : chip->client_group_b;
+	ret = i2c_smbus_write_byte(client, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writing\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max732x_read(struct max732x_chip *chip, int group_a, uint8_t *val)
+{
+	struct i2c_client *client;
+	int ret;
+
+	client = group_a ? chip->client_group_a : chip->client_group_b;
+	ret = i2c_smbus_read_byte(client);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading\n");
+		return ret;
+	}
+
+	*val = (uint8_t)ret;
+	return 0;
+}
+
+static inline int is_group_a(struct max732x_chip *chip, unsigned off)
+{
+	return (1u << off) & chip->mask_group_a;
+}
+
+static int max732x_gpio_get_value(struct gpio_chip *gc, unsigned off)
+{
+	struct max732x_chip *chip;
+	uint8_t reg_val;
+	int ret;
+
+	chip = container_of(gc, struct max732x_chip, gpio_chip);
+
+	ret = max732x_read(chip, is_group_a(chip, off), &reg_val);
+	if (ret < 0)
+		return 0;
+
+	return reg_val & (1u << (off & 0x7));
+}
+
+static void max732x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
+{
+	struct max732x_chip *chip;
+	uint8_t reg_out, mask = 1u << (off & 0x7);
+	int ret;
+
+	chip = container_of(gc, struct max732x_chip, gpio_chip);
+
+	mutex_lock(&chip->lock);
+
+	reg_out = (off > 7) ? chip->reg_out[1] : chip->reg_out[0];
+	reg_out = (val) ? reg_out | mask : reg_out & ~mask;
+
+	ret = max732x_write(chip, is_group_a(chip, off), reg_out);
+	if (ret < 0)
+		goto out;
+
+	/* update the shadow register then */
+	if (off > 7)
+		chip->reg_out[1] = reg_out;
+	else
+		chip->reg_out[0] = reg_out;
+out:
+	mutex_unlock(&chip->lock);
+}
+
+static int max732x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
+{
+	struct max732x_chip *chip;
+	unsigned int mask = 1u << off;
+
+	chip = container_of(gc, struct max732x_chip, gpio_chip);
+
+	if ((mask & chip->dir_input) == 0) {
+		dev_dbg(&chip->client->dev, "%s port %d is output only\n",
+			chip->client->name, off);
+		return -EACCES;
+	}
+
+	return 0;
+}
+
+static int max732x_gpio_direction_output(struct gpio_chip *gc,
+		unsigned off, int val)
+{
+	struct max732x_chip *chip;
+	unsigned int mask = 1u << off;
+
+	chip = container_of(gc, struct max732x_chip, gpio_chip);
+
+	if ((mask & chip->dir_output) == 0) {
+		dev_dbg(&chip->client->dev, "%s port %d is input only\n",
+			chip->client->name, off);
+		return -EACCES;
+	}
+
+	max732x_gpio_set_value(gc, off, val);
+	return 0;
+}
+
+static int __devinit max732x_setup_gpio(struct max732x_chip *chip,
+					const struct i2c_device_id *id,
+					unsigned gpio_start)
+{
+	struct gpio_chip *gc = &chip->gpio_chip;
+	uint32_t id_data = id->driver_data;
+	int i, port = 0;
+
+	for (i = 0; i < 16; i++, id_data >>= 2) {
+		unsigned int mask = 1 << port;
+
+		switch (id_data & 0x3) {
+		case PORT_OUTPUT:
+			chip->dir_output |= mask;
+			break;
+		case PORT_INPUT:
+			chip->dir_input |= mask;
+			break;
+		case PORT_OPENDRAIN:
+			chip->dir_output |= mask;
+			chip->dir_input |= mask;
+			break;
+		default:
+			continue;
+		}
+
+		if (i < 8)
+			chip->mask_group_a |= mask;
+		port++;
+	}
+
+	if (chip->dir_input)
+		gc->direction_input = max732x_gpio_direction_input;
+	if (chip->dir_output) {
+		gc->direction_output = max732x_gpio_direction_output;
+		gc->set = max732x_gpio_set_value;
+	}
+	gc->get = max732x_gpio_get_value;
+	gc->can_sleep = 1;
+
+	gc->base = gpio_start;
+	gc->ngpio = port;
+	gc->label = chip->client->name;
+	gc->owner = THIS_MODULE;
+
+	return port;
+}
+
+static int __devinit max732x_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	struct max732x_platform_data *pdata;
+	struct max732x_chip *chip;
+	struct i2c_client *c;
+	uint16_t addr_a, addr_b;
+	int ret, nr_port;
+
+	pdata = client->dev.platform_data;
+	if (pdata == NULL)
+		return -ENODEV;
+
+	chip = kzalloc(sizeof(struct max732x_chip), GFP_KERNEL);
+	if (chip == NULL)
+		return -ENOMEM;
+	chip->client = client;
+
+	nr_port = max732x_setup_gpio(chip, id, pdata->gpio_base);
+
+	addr_a = (client->addr & 0x0f) | 0x60;
+	addr_b = (client->addr & 0x0f) | 0x50;
+
+	switch (client->addr & 0x70) {
+	case 0x60:
+		chip->client_group_a = client;
+		if (nr_port > 7) {
+			c = i2c_new_dummy(client->adapter, addr_b);
+			chip->client_group_b = chip->client_dummy = c;
+		}
+		break;
+	case 0x50:
+		chip->client_group_b = client;
+		if (nr_port > 7) {
+			c = i2c_new_dummy(client->adapter, addr_a);
+			chip->client_group_a = chip->client_dummy = c;
+		}
+		break;
+	default:
+		dev_err(&client->dev, "invalid I2C address specified %02x\n",
+				client->addr);
+		ret = -EINVAL;
+		goto out_failed;
+	}
+
+	mutex_init(&chip->lock);
+
+	max732x_read(chip, is_group_a(chip, 0), &chip->reg_out[0]);
+	if (nr_port > 7)
+		max732x_read(chip, is_group_a(chip, 8), &chip->reg_out[1]);
+
+	ret = gpiochip_add(&chip->gpio_chip);
+	if (ret)
+		goto out_failed;
+
+	if (pdata->setup) {
+		ret = pdata->setup(client, chip->gpio_chip.base,
+				chip->gpio_chip.ngpio, pdata->context);
+		if (ret < 0)
+			dev_warn(&client->dev, "setup failed, %d\n", ret);
+	}
+
+	i2c_set_clientdata(client, chip);
+	return 0;
+
+out_failed:
+	kfree(chip);
+	return ret;
+}
+
+static int __devexit max732x_remove(struct i2c_client *client)
+{
+	struct max732x_platform_data *pdata = client->dev.platform_data;
+	struct max732x_chip *chip = i2c_get_clientdata(client);
+	int ret;
+
+	if (pdata->teardown) {
+		ret = pdata->teardown(client, chip->gpio_chip.base,
+				chip->gpio_chip.ngpio, pdata->context);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s failed, %d\n",
+					"teardown", ret);
+			return ret;
+		}
+	}
+
+	ret = gpiochip_remove(&chip->gpio_chip);
+	if (ret) {
+		dev_err(&client->dev, "%s failed, %d\n",
+				"gpiochip_remove()", ret);
+		return ret;
+	}
+
+	/* unregister any dummy i2c_client */
+	if (chip->client_dummy)
+		i2c_unregister_device(chip->client_dummy);
+
+	kfree(chip);
+	return 0;
+}
+
+static struct i2c_driver max732x_driver = {
+	.driver = {
+		.name	= "max732x",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max732x_probe,
+	.remove		= __devexit_p(max732x_remove),
+	.id_table	= max732x_id,
+};
+
+static int __init max732x_init(void)
+{
+	return i2c_add_driver(&max732x_driver);
+}
+module_init(max732x_init);
+
+static void __exit max732x_exit(void)
+{
+	i2c_del_driver(&max732x_driver);
+}
+module_exit(max732x_exit);
+
+MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>");
+MODULE_DESCRIPTION("GPIO expander driver for MAX732X");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/mcp23s08.c b/drivers/gpio/mcp23s08.c
index 7f92fdd5f0e..8a1b405fefd 100644
--- a/drivers/gpio/mcp23s08.c
+++ b/drivers/gpio/mcp23s08.c
@@ -40,15 +40,26 @@ struct mcp23s08 {
 	struct spi_device	*spi;
 	u8			addr;
 
+	u8			cache[11];
 	/* lock protects the cached values */
 	struct mutex		lock;
-	u8			cache[11];
 
 	struct gpio_chip	chip;
 
 	struct work_struct	work;
 };
 
+/* A given spi_device can represent up to four mcp23s08 chips
+ * sharing the same chipselect but using different addresses
+ * (e.g. chips #0 and #3 might be populated, but not #1 or $2).
+ * Driver data holds all the per-chip data.
+ */
+struct mcp23s08_driver_data {
+	unsigned		ngpio;
+	struct mcp23s08		*mcp[4];
+	struct mcp23s08		chip[];
+};
+
 static int mcp23s08_read(struct mcp23s08 *mcp, unsigned reg)
 {
 	u8	tx[2], rx[1];
@@ -208,25 +219,18 @@ done:
 
 /*----------------------------------------------------------------------*/
 
-static int mcp23s08_probe(struct spi_device *spi)
+static int mcp23s08_probe_one(struct spi_device *spi, unsigned addr,
+		unsigned base, unsigned pullups)
 {
-	struct mcp23s08			*mcp;
-	struct mcp23s08_platform_data	*pdata;
+	struct mcp23s08_driver_data	*data = spi_get_drvdata(spi);
+	struct mcp23s08			*mcp = data->mcp[addr];
 	int				status;
 	int				do_update = 0;
 
-	pdata = spi->dev.platform_data;
-	if (!pdata || pdata->slave > 3 || !pdata->base)
-		return -ENODEV;
-
-	mcp = kzalloc(sizeof *mcp, GFP_KERNEL);
-	if (!mcp)
-		return -ENOMEM;
-
 	mutex_init(&mcp->lock);
 
 	mcp->spi = spi;
-	mcp->addr = 0x40 | (pdata->slave << 1);
+	mcp->addr = 0x40 | (addr << 1);
 
 	mcp->chip.label = "mcp23s08",
 
@@ -236,26 +240,28 @@ static int mcp23s08_probe(struct spi_device *spi)
 	mcp->chip.set = mcp23s08_set;
 	mcp->chip.dbg_show = mcp23s08_dbg_show;
 
-	mcp->chip.base = pdata->base;
+	mcp->chip.base = base;
 	mcp->chip.ngpio = 8;
 	mcp->chip.can_sleep = 1;
+	mcp->chip.dev = &spi->dev;
 	mcp->chip.owner = THIS_MODULE;
 
-	spi_set_drvdata(spi, mcp);
-
-	/* verify MCP_IOCON.SEQOP = 0, so sequential reads work */
+	/* verify MCP_IOCON.SEQOP = 0, so sequential reads work,
+	 * and MCP_IOCON.HAEN = 1, so we work with all chips.
+	 */
 	status = mcp23s08_read(mcp, MCP_IOCON);
 	if (status < 0)
 		goto fail;
-	if (status & IOCON_SEQOP) {
+	if ((status & IOCON_SEQOP) || !(status & IOCON_HAEN)) {
 		status &= ~IOCON_SEQOP;
+		status |= IOCON_HAEN;
 		status = mcp23s08_write(mcp, MCP_IOCON, (u8) status);
 		if (status < 0)
 			goto fail;
 	}
 
 	/* configure ~100K pullups */
-	status = mcp23s08_write(mcp, MCP_GPPU, pdata->pullups);
+	status = mcp23s08_write(mcp, MCP_GPPU, pullups);
 	if (status < 0)
 		goto fail;
 
@@ -282,11 +288,58 @@ static int mcp23s08_probe(struct spi_device *spi)
 		tx[1] = MCP_IPOL;
 		memcpy(&tx[2], &mcp->cache[MCP_IPOL], sizeof(tx) - 2);
 		status = spi_write_then_read(mcp->spi, tx, sizeof tx, NULL, 0);
-
-		/* FIXME check status... */
+		if (status < 0)
+			goto fail;
 	}
 
 	status = gpiochip_add(&mcp->chip);
+fail:
+	if (status < 0)
+		dev_dbg(&spi->dev, "can't setup chip %d, --> %d\n",
+				addr, status);
+	return status;
+}
+
+static int mcp23s08_probe(struct spi_device *spi)
+{
+	struct mcp23s08_platform_data	*pdata;
+	unsigned			addr;
+	unsigned			chips = 0;
+	struct mcp23s08_driver_data	*data;
+	int				status;
+	unsigned			base;
+
+	pdata = spi->dev.platform_data;
+	if (!pdata || !gpio_is_valid(pdata->base))
+		return -ENODEV;
+
+	for (addr = 0; addr < 4; addr++) {
+		if (!pdata->chip[addr].is_present)
+			continue;
+		chips++;
+	}
+	if (!chips)
+		return -ENODEV;
+
+	data = kzalloc(sizeof *data + chips * sizeof(struct mcp23s08),
+			GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	spi_set_drvdata(spi, data);
+
+	base = pdata->base;
+	for (addr = 0; addr < 4; addr++) {
+		if (!pdata->chip[addr].is_present)
+			continue;
+		chips--;
+		data->mcp[addr] = &data->chip[chips];
+		status = mcp23s08_probe_one(spi, addr, base,
+				pdata->chip[addr].pullups);
+		if (status < 0)
+			goto fail;
+		base += 8;
+	}
+	data->ngpio = base - pdata->base;
 
 	/* NOTE:  these chips have a relatively sane IRQ framework, with
 	 * per-signal masking and level/edge triggering.  It's not yet
@@ -294,8 +347,9 @@ static int mcp23s08_probe(struct spi_device *spi)
 	 */
 
 	if (pdata->setup) {
-		status = pdata->setup(spi, mcp->chip.base,
-				mcp->chip.ngpio, pdata->context);
+		status = pdata->setup(spi,
+				pdata->base, data->ngpio,
+				pdata->context);
 		if (status < 0)
 			dev_dbg(&spi->dev, "setup --> %d\n", status);
 	}
@@ -303,19 +357,29 @@ static int mcp23s08_probe(struct spi_device *spi)
 	return 0;
 
 fail:
-	kfree(mcp);
+	for (addr = 0; addr < 4; addr++) {
+		int tmp;
+
+		if (!data->mcp[addr])
+			continue;
+		tmp = gpiochip_remove(&data->mcp[addr]->chip);
+		if (tmp < 0)
+			dev_err(&spi->dev, "%s --> %d\n", "remove", tmp);
+	}
+	kfree(data);
 	return status;
 }
 
 static int mcp23s08_remove(struct spi_device *spi)
 {
-	struct mcp23s08			*mcp = spi_get_drvdata(spi);
+	struct mcp23s08_driver_data	*data = spi_get_drvdata(spi);
 	struct mcp23s08_platform_data	*pdata = spi->dev.platform_data;
+	unsigned			addr;
 	int				status = 0;
 
 	if (pdata->teardown) {
 		status = pdata->teardown(spi,
-				mcp->chip.base, mcp->chip.ngpio,
+				pdata->base, data->ngpio,
 				pdata->context);
 		if (status < 0) {
 			dev_err(&spi->dev, "%s --> %d\n", "teardown", status);
@@ -323,11 +387,20 @@ static int mcp23s08_remove(struct spi_device *spi)
 		}
 	}
 
-	status = gpiochip_remove(&mcp->chip);
+	for (addr = 0; addr < 4; addr++) {
+		int tmp;
+
+		if (!data->mcp[addr])
+			continue;
+
+		tmp = gpiochip_remove(&data->mcp[addr]->chip);
+		if (tmp < 0) {
+			dev_err(&spi->dev, "%s --> %d\n", "remove", tmp);
+			status = tmp;
+		}
+	}
 	if (status == 0)
-		kfree(mcp);
-	else
-		dev_err(&spi->dev, "%s --> %d\n", "remove", status);
+		kfree(data);
 	return status;
 }
 
@@ -355,4 +428,3 @@ static void __exit mcp23s08_exit(void)
 module_exit(mcp23s08_exit);
 
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/gpio/pca953x.c b/drivers/gpio/pca953x.c
index a380730b61a..cc8468692ae 100644
--- a/drivers/gpio/pca953x.c
+++ b/drivers/gpio/pca953x.c
@@ -188,6 +188,7 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 	gc->base = chip->gpio_start;
 	gc->ngpio = gpios;
 	gc->label = chip->client->name;
+	gc->dev = &chip->client->dev;
 	gc->owner = THIS_MODULE;
 }
 
diff --git a/drivers/gpio/pcf857x.c b/drivers/gpio/pcf857x.c
index d25d356c4f2..fc9c6ae739e 100644
--- a/drivers/gpio/pcf857x.c
+++ b/drivers/gpio/pcf857x.c
@@ -200,6 +200,7 @@ static int pcf857x_probe(struct i2c_client *client,
 
 	gpio->chip.base = pdata->gpio_base;
 	gpio->chip.can_sleep = 1;
+	gpio->chip.dev = &client->dev;
 	gpio->chip.owner = THIS_MODULE;
 
 	/* NOTE:  the OnSemi jlc1562b is also largely compatible with
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 564138714bb..452c2d866ec 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -318,7 +318,7 @@ static void drm_cleanup(struct drm_device * dev)
 		DRM_ERROR("Cannot unload module\n");
 }
 
-int drm_minors_cleanup(int id, void *ptr, void *data)
+static int drm_minors_cleanup(int id, void *ptr, void *data)
 {
 	struct drm_minor *minor = ptr;
 	struct drm_device *dev;
diff --git a/drivers/i2c/chips/Kconfig b/drivers/i2c/chips/Kconfig
index 50e0a465374..a95cb9465d6 100644
--- a/drivers/i2c/chips/Kconfig
+++ b/drivers/i2c/chips/Kconfig
@@ -126,7 +126,7 @@ config ISP1301_OMAP
 
 config TPS65010
 	tristate "TPS6501x Power Management chips"
-	depends on HAVE_GPIO_LIB
+	depends on GPIOLIB
 	default y if MACH_OMAP_H2 || MACH_OMAP_H3 || MACH_OMAP_OSK
 	help
 	  If you say yes here you get support for the TPS6501x series of
diff --git a/drivers/i2c/chips/tps65010.c b/drivers/i2c/chips/tps65010.c
index 85949685191..cf02e8fceb4 100644
--- a/drivers/i2c/chips/tps65010.c
+++ b/drivers/i2c/chips/tps65010.c
@@ -636,6 +636,8 @@ static int tps65010_probe(struct i2c_client *client,
 		tps->outmask = board->outmask;
 
 		tps->chip.label = client->name;
+		tps->chip.dev = &client->dev;
+		tps->chip.owner = THIS_MODULE;
 
 		tps->chip.set = tps65010_gpio_set;
 		tps->chip.direction_output = tps65010_output;
diff --git a/drivers/ide/Kconfig b/drivers/ide/Kconfig
index 04d9c4d459d..130ef64b44f 100644
--- a/drivers/ide/Kconfig
+++ b/drivers/ide/Kconfig
@@ -314,7 +314,7 @@ comment "IDE chipset support/bugfixes"
 
 config IDE_GENERIC
 	tristate "generic/default IDE chipset support"
-	depends on ALPHA || X86 || IA64 || M32R || MIPS || PPC32
+	depends on ALPHA || X86 || IA64 || M32R || MIPS
 	help
 	  If unsure, say N.
 
diff --git a/drivers/ide/Makefile b/drivers/ide/Makefile
index 5d414e301a5..64e0ecdc4ed 100644
--- a/drivers/ide/Makefile
+++ b/drivers/ide/Makefile
@@ -1,13 +1,6 @@
 #
-# Makefile for the kernel ata, atapi, and ide block device drivers.
-#
-# 12 September 2000, Bartlomiej Zolnierkiewicz <bkz@linux-ide.org>
-# Rewritten to use lists instead of if-statements.
-#
-# Note : at this point, these files are compiled on all systems.
-# In the future, some of these should be built conditionally.
-#
 # link order is important here
+#
 
 EXTRA_CFLAGS				+= -Idrivers/ide
 
diff --git a/drivers/ide/arm/icside.c b/drivers/ide/arm/icside.c
index f575e8341ae..df4af408395 100644
--- a/drivers/ide/arm/icside.c
+++ b/drivers/ide/arm/icside.c
@@ -710,8 +710,14 @@ static int __init icside_init(void)
 	return ecard_register_driver(&icside_driver);
 }
 
+static void __exit icside_exit(void);
+{
+	ecard_unregister_driver(&icside_driver);
+}
+
 MODULE_AUTHOR("Russell King <rmk@arm.linux.org.uk>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("ICS IDE driver");
 
 module_init(icside_init);
+module_exit(icside_exit);
diff --git a/drivers/ide/arm/palm_bk3710.c b/drivers/ide/arm/palm_bk3710.c
index 65bb4b8fd57..3e842d60eae 100644
--- a/drivers/ide/arm/palm_bk3710.c
+++ b/drivers/ide/arm/palm_bk3710.c
@@ -82,6 +82,7 @@ static const struct palm_bk3710_udmatiming palm_bk3710_udmatimings[6] = {
 	{100, 120},		/* UDMA Mode 2 */
 	{100, 90},		/* UDMA Mode 3 */
 	{100, 60},		/* UDMA Mode 4 */
+	{85,  40},		/* UDMA Mode 5 */
 };
 
 static void palm_bk3710_setudmamode(void __iomem *base, unsigned int dev,
@@ -334,12 +335,11 @@ static const struct ide_port_ops palm_bk3710_ports_ops = {
 	.cable_detect		= palm_bk3710_cable_detect,
 };
 
-static const struct ide_port_info __devinitdata palm_bk3710_port_info = {
+static struct ide_port_info __devinitdata palm_bk3710_port_info = {
 	.init_dma		= palm_bk3710_init_dma,
 	.port_ops		= &palm_bk3710_ports_ops,
 	.host_flags		= IDE_HFLAG_MMIO,
 	.pio_mask		= ATA_PIO4,
-	.udma_mask		= ATA_UDMA4,	/* (input clk 99MHz) */
 	.mwdma_mask		= ATA_MWDMA2,
 };
 
@@ -352,7 +352,7 @@ static int __devinit palm_bk3710_probe(struct platform_device *pdev)
 	int i, rc;
 	hw_regs_t hw, *hws[] = { &hw, NULL, NULL, NULL };
 
-	clk = clk_get(NULL, "IDECLK");
+	clk = clk_get(&pdev->dev, "IDECLK");
 	if (IS_ERR(clk))
 		return -ENODEV;
 
@@ -392,6 +392,9 @@ static int __devinit palm_bk3710_probe(struct platform_device *pdev)
 	hw.irq = irq->start;
 	hw.chipset = ide_palm3710;
 
+	palm_bk3710_port_info.udma_mask = rate < 100000000 ? ATA_UDMA4 :
+							     ATA_UDMA5;
+
 	rc = ide_host_add(&palm_bk3710_port_info, hws, NULL);
 	if (rc)
 		goto out;
diff --git a/drivers/ide/arm/rapide.c b/drivers/ide/arm/rapide.c
index 2bdd8b734af..78d27d9ae43 100644
--- a/drivers/ide/arm/rapide.c
+++ b/drivers/ide/arm/rapide.c
@@ -95,7 +95,13 @@ static int __init rapide_init(void)
 	return ecard_register_driver(&rapide_driver);
 }
 
+static void __exit rapide_exit(void)
+{
+	ecard_unregister_driver(&rapide_driver);
+}
+
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Yellowstone RAPIDE driver");
 
 module_init(rapide_init);
+module_exit(rapide_exit);
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 4e73aeee405..e617cf08aef 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -57,23 +57,29 @@ static DEFINE_MUTEX(idecd_ref_mutex);
 #define ide_cd_g(disk) \
 	container_of((disk)->private_data, struct cdrom_info, driver)
 
+static void ide_cd_release(struct kref *);
+
 static struct cdrom_info *ide_cd_get(struct gendisk *disk)
 {
 	struct cdrom_info *cd = NULL;
 
 	mutex_lock(&idecd_ref_mutex);
 	cd = ide_cd_g(disk);
-	if (cd)
+	if (cd) {
 		kref_get(&cd->kref);
+		if (ide_device_get(cd->drive)) {
+			kref_put(&cd->kref, ide_cd_release);
+			cd = NULL;
+		}
+	}
 	mutex_unlock(&idecd_ref_mutex);
 	return cd;
 }
 
-static void ide_cd_release(struct kref *);
-
 static void ide_cd_put(struct cdrom_info *cd)
 {
 	mutex_lock(&idecd_ref_mutex);
+	ide_device_put(cd->drive);
 	kref_put(&cd->kref, ide_cd_release);
 	mutex_unlock(&idecd_ref_mutex);
 }
@@ -1305,13 +1311,30 @@ static int cdrom_read_capacity(ide_drive_t *drive, unsigned long *capacity,
 
 	stat = ide_cd_queue_pc(drive, cmd, 0, &capbuf, &len, sense, 0,
 			       REQ_QUIET);
-	if (stat == 0) {
-		*capacity = 1 + be32_to_cpu(capbuf.lba);
-		*sectors_per_frame =
-			be32_to_cpu(capbuf.blocklen) >> SECTOR_BITS;
+	if (stat)
+		return stat;
+
+	/*
+	 * Sanity check the given block size
+	 */
+	switch (capbuf.blocklen) {
+	case __constant_cpu_to_be32(512):
+	case __constant_cpu_to_be32(1024):
+	case __constant_cpu_to_be32(2048):
+	case __constant_cpu_to_be32(4096):
+		break;
+	default:
+		printk(KERN_ERR "%s: weird block size %u\n",
+			drive->name, capbuf.blocklen);
+		printk(KERN_ERR "%s: default to 2kb block size\n",
+			drive->name);
+		capbuf.blocklen = __constant_cpu_to_be32(2048);
+		break;
 	}
 
-	return stat;
+	*capacity = 1 + be32_to_cpu(capbuf.lba);
+	*sectors_per_frame = be32_to_cpu(capbuf.blocklen) >> SECTOR_BITS;
+	return 0;
 }
 
 static int cdrom_read_tocentry(ide_drive_t *drive, int trackno, int msf_flag,
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index df5fe575687..28d85b410f7 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -56,23 +56,29 @@ static DEFINE_MUTEX(idedisk_ref_mutex);
 #define ide_disk_g(disk) \
 	container_of((disk)->private_data, struct ide_disk_obj, driver)
 
+static void ide_disk_release(struct kref *);
+
 static struct ide_disk_obj *ide_disk_get(struct gendisk *disk)
 {
 	struct ide_disk_obj *idkp = NULL;
 
 	mutex_lock(&idedisk_ref_mutex);
 	idkp = ide_disk_g(disk);
-	if (idkp)
+	if (idkp) {
 		kref_get(&idkp->kref);
+		if (ide_device_get(idkp->drive)) {
+			kref_put(&idkp->kref, ide_disk_release);
+			idkp = NULL;
+		}
+	}
 	mutex_unlock(&idedisk_ref_mutex);
 	return idkp;
 }
 
-static void ide_disk_release(struct kref *);
-
 static void ide_disk_put(struct ide_disk_obj *idkp)
 {
 	mutex_lock(&idedisk_ref_mutex);
+	ide_device_put(idkp->drive);
 	kref_put(&idkp->kref, ide_disk_release);
 	mutex_unlock(&idedisk_ref_mutex);
 }
diff --git a/drivers/ide/ide-dma.c b/drivers/ide/ide-dma.c
index be99d463dcc..71c377a7bcf 100644
--- a/drivers/ide/ide-dma.c
+++ b/drivers/ide/ide-dma.c
@@ -173,7 +173,7 @@ EXPORT_SYMBOL_GPL(ide_build_sglist);
 int ide_build_dmatable (ide_drive_t *drive, struct request *rq)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
-	unsigned int *table	= hwif->dmatable_cpu;
+	__le32 *table = (__le32 *)hwif->dmatable_cpu;
 	unsigned int is_trm290	= (hwif->chipset == ide_trm290) ? 1 : 0;
 	unsigned int count = 0;
 	int i;
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 3d8e6dd0f41..ca11a26746f 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -158,23 +158,29 @@ static DEFINE_MUTEX(idefloppy_ref_mutex);
 #define ide_floppy_g(disk) \
 	container_of((disk)->private_data, struct ide_floppy_obj, driver)
 
+static void idefloppy_cleanup_obj(struct kref *);
+
 static struct ide_floppy_obj *ide_floppy_get(struct gendisk *disk)
 {
 	struct ide_floppy_obj *floppy = NULL;
 
 	mutex_lock(&idefloppy_ref_mutex);
 	floppy = ide_floppy_g(disk);
-	if (floppy)
+	if (floppy) {
 		kref_get(&floppy->kref);
+		if (ide_device_get(floppy->drive)) {
+			kref_put(&floppy->kref, idefloppy_cleanup_obj);
+			floppy = NULL;
+		}
+	}
 	mutex_unlock(&idefloppy_ref_mutex);
 	return floppy;
 }
 
-static void idefloppy_cleanup_obj(struct kref *);
-
 static void ide_floppy_put(struct ide_floppy_obj *floppy)
 {
 	mutex_lock(&idefloppy_ref_mutex);
+	ide_device_put(floppy->drive);
 	kref_put(&floppy->kref, idefloppy_cleanup_obj);
 	mutex_unlock(&idefloppy_ref_mutex);
 }
diff --git a/drivers/ide/ide-generic.c b/drivers/ide/ide-generic.c
index 31d98fec775..8fe8b5b9cf7 100644
--- a/drivers/ide/ide-generic.c
+++ b/drivers/ide/ide-generic.c
@@ -20,6 +20,11 @@
 #include <linux/module.h>
 #include <linux/ide.h>
 
+/* FIXME: convert m32r to use ide_platform host driver */
+#ifdef CONFIG_M32R
+#include <asm/m32r.h>
+#endif
+
 #define DRV_NAME	"ide_generic"
 
 static int probe_mask = 0x03;
@@ -80,6 +85,21 @@ static int __init ide_generic_sysfs_init(void)
 	return 0;
 }
 
+#if defined(CONFIG_PLAT_M32700UT) || defined(CONFIG_PLAT_MAPPI2) \
+	|| defined(CONFIG_PLAT_OPSPUT)
+static const u16 legacy_bases[] = { 0x1f0 };
+static const int legacy_irqs[]  = { PLD_IRQ_CFIREQ };
+#elif defined(CONFIG_PLAT_MAPPI3)
+static const u16 legacy_bases[] = { 0x1f0, 0x170 };
+static const int legacy_irqs[]  = { PLD_IRQ_CFIREQ, PLD_IRQ_IDEIREQ };
+#elif defined(CONFIG_ALPHA)
+static const u16 legacy_bases[] = { 0x1f0, 0x170, 0x1e8, 0x168 };
+static const int legacy_irqs[]  = { 14, 15, 11, 10 };
+#else
+static const u16 legacy_bases[] = { 0x1f0, 0x170, 0x1e8, 0x168, 0x1e0, 0x160 };
+static const int legacy_irqs[]  = { 14, 15, 11, 10, 8, 12 };
+#endif
+
 static int __init ide_generic_init(void)
 {
 	hw_regs_t hw[MAX_HWIFS], *hws[MAX_HWIFS];
@@ -87,11 +107,17 @@ static int __init ide_generic_init(void)
 	unsigned long io_addr;
 	int i, rc;
 
+#ifdef CONFIG_MIPS
+	if (!ide_probe_legacy())
+		return -ENODEV;
+#endif
 	printk(KERN_INFO DRV_NAME ": please use \"probe_mask=0x3f\" module "
 			 "parameter for probing all legacy ISA IDE ports\n");
 
-	for (i = 0; i < MAX_HWIFS; i++) {
-		io_addr = ide_default_io_base(i);
+	memset(hws, 0, sizeof(hw_regs_t *) * MAX_HWIFS);
+
+	for (i = 0; i < ARRAY_SIZE(legacy_bases); i++) {
+		io_addr = legacy_bases[i];
 
 		hws[i] = NULL;
 
@@ -113,7 +139,11 @@ static int __init ide_generic_init(void)
 
 			memset(&hw[i], 0, sizeof(hw[i]));
 			ide_std_init_ports(&hw[i], io_addr, io_addr + 0x206);
-			hw[i].irq = ide_default_irq(io_addr);
+#ifdef CONFIG_IA64
+			hw[i].irq = isa_irq_to_vector(legacy_irqs[i]);
+#else
+			hw[i].irq = legacy_irqs[i];
+#endif
 			hw[i].chipset = ide_generic;
 
 			hws[i] = &hw[i];
diff --git a/drivers/ide/ide-iops.c b/drivers/ide/ide-iops.c
index 07da5fb9eaf..8aae9176451 100644
--- a/drivers/ide/ide-iops.c
+++ b/drivers/ide/ide-iops.c
@@ -510,10 +510,8 @@ void ide_fixstring (u8 *s, const int bytecount, const int byteswap)
 
 	if (byteswap) {
 		/* convert from big-endian to host byte order */
-		for (p = end ; p != s;) {
-			unsigned short *pp = (unsigned short *) (p -= 2);
-			*pp = ntohs(*pp);
-		}
+		for (p = end ; p != s;)
+			be16_to_cpus((u16 *)(p -= 2));
 	}
 	/* strip leading blanks */
 	while (s != end && *s == ' ')
diff --git a/drivers/ide/ide-probe.c b/drivers/ide/ide-probe.c
index 4aa76c45375..994e41099b4 100644
--- a/drivers/ide/ide-probe.c
+++ b/drivers/ide/ide-probe.c
@@ -134,18 +134,6 @@ static inline void do_identify (ide_drive_t *drive, u8 cmd)
 #endif
 	ide_fix_driveid(id);
 
-#if defined (CONFIG_SCSI_EATA_PIO) || defined (CONFIG_SCSI_EATA)
-	/*
-	 * EATA SCSI controllers do a hardware ATA emulation:
-	 * Ignore them if there is a driver for them available.
-	 */
-	if ((id->model[0] == 'P' && id->model[1] == 'M') ||
-	    (id->model[0] == 'S' && id->model[1] == 'K')) {
-		printk("%s: EATA SCSI HBA %.10s\n", drive->name, id->model);
-		goto err_misc;
-	}
-#endif /* CONFIG_SCSI_EATA || CONFIG_SCSI_EATA_PIO */
-
 	/*
 	 *  WIN_IDENTIFY returns little-endian info,
 	 *  WIN_PIDENTIFY *usually* returns little-endian info.
@@ -167,7 +155,8 @@ static inline void do_identify (ide_drive_t *drive, u8 cmd)
 	if (strstr(id->model, "E X A B Y T E N E S T"))
 		goto err_misc;
 
-	printk("%s: %s, ", drive->name, id->model);
+	printk(KERN_INFO "%s: %s, ", drive->name, id->model);
+
 	drive->present = 1;
 	drive->dead = 0;
 
@@ -176,16 +165,17 @@ static inline void do_identify (ide_drive_t *drive, u8 cmd)
 	 */
 	if (cmd == WIN_PIDENTIFY) {
 		u8 type = (id->config >> 8) & 0x1f;
-		printk("ATAPI ");
+
+		printk(KERN_CONT "ATAPI ");
 		switch (type) {
 			case ide_floppy:
 				if (!strstr(id->model, "CD-ROM")) {
 					if (!strstr(id->model, "oppy") &&
 					    !strstr(id->model, "poyp") &&
 					    !strstr(id->model, "ZIP"))
-						printk("cdrom or floppy?, assuming ");
+						printk(KERN_CONT "cdrom or floppy?, assuming ");
 					if (drive->media != ide_cdrom) {
-						printk ("FLOPPY");
+						printk(KERN_CONT "FLOPPY");
 						drive->removable = 1;
 						break;
 					}
@@ -198,25 +188,25 @@ static inline void do_identify (ide_drive_t *drive, u8 cmd)
 				/* kludge for Apple PowerBook internal zip */
 				if (!strstr(id->model, "CD-ROM") &&
 				    strstr(id->model, "ZIP")) {
-					printk ("FLOPPY");
+					printk(KERN_CONT "FLOPPY");
 					type = ide_floppy;
 					break;
 				}
 #endif
-				printk ("CD/DVD-ROM");
+				printk(KERN_CONT "CD/DVD-ROM");
 				break;
 			case ide_tape:
-				printk ("TAPE");
+				printk(KERN_CONT "TAPE");
 				break;
 			case ide_optical:
-				printk ("OPTICAL");
+				printk(KERN_CONT "OPTICAL");
 				drive->removable = 1;
 				break;
 			default:
-				printk("UNKNOWN (type %d)", type);
+				printk(KERN_CONT "UNKNOWN (type %d)", type);
 				break;
 		}
-		printk (" drive\n");
+		printk(KERN_CONT " drive\n");
 		drive->media = type;
 		/* an ATAPI device ignores DRDY */
 		drive->ready_stat = 0;
@@ -236,7 +226,9 @@ static inline void do_identify (ide_drive_t *drive, u8 cmd)
 		drive->removable = 1;
 
 	drive->media = ide_disk;
-	printk("%s DISK drive\n", (id->config == 0x848a) ? "CFA" : "ATA" );
+
+	printk(KERN_CONT "%s DISK drive\n",
+		(id->config == 0x848a) ? "CFA" : "ATA");
 
 	return;
 
@@ -387,7 +379,7 @@ static int try_to_identify (ide_drive_t *drive, u8 cmd)
 				/* Mmmm.. multiple IRQs..
 				 * don't know which was ours
 				 */
-				printk("%s: IRQ probe failed (0x%lx)\n",
+				printk(KERN_ERR "%s: IRQ probe failed (0x%lx)\n",
 					drive->name, cookie);
 			}
 		}
@@ -456,7 +448,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
 			return 4;
 	}
 #ifdef DEBUG
-	printk("probing for %s: present=%d, media=%d, probetype=%s\n",
+	printk(KERN_INFO "probing for %s: present=%d, media=%d, probetype=%s\n",
 		drive->name, drive->present, drive->media,
 		(cmd == WIN_IDENTIFY) ? "ATA" : "ATAPI");
 #endif
@@ -534,7 +526,8 @@ static void enable_nest (ide_drive_t *drive)
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	u8 stat;
 
-	printk("%s: enabling %s -- ", hwif->name, drive->id->model);
+	printk(KERN_INFO "%s: enabling %s -- ", hwif->name, drive->id->model);
+
 	SELECT_DRIVE(drive);
 	msleep(50);
 	tp_ops->exec_command(hwif, EXABYTE_ENABLE_NEST);
@@ -883,7 +876,7 @@ static void save_match(ide_hwif_t *hwif, ide_hwif_t *new, ide_hwif_t **match)
 	if (m && m->hwgroup && m->hwgroup != new->hwgroup) {
 		if (!new->hwgroup)
 			return;
-		printk("%s: potential irq problem with %s and %s\n",
+		printk(KERN_WARNING "%s: potential IRQ problem with %s and %s\n",
 			hwif->name, new->name, m->name);
 	}
 	if (!m || m->irq != hwif->irq) /* don't undo a prior perfect match */
@@ -1142,17 +1135,17 @@ static int init_irq (ide_hwif_t *hwif)
 	}
 
 #if !defined(__mc68000__)
-	printk("%s at 0x%03lx-0x%03lx,0x%03lx on irq %d", hwif->name,
+	printk(KERN_INFO "%s at 0x%03lx-0x%03lx,0x%03lx on irq %d", hwif->name,
 		io_ports->data_addr, io_ports->status_addr,
 		io_ports->ctl_addr, hwif->irq);
 #else
-	printk("%s at 0x%08lx on irq %d", hwif->name,
+	printk(KERN_INFO "%s at 0x%08lx on irq %d", hwif->name,
 		io_ports->data_addr, hwif->irq);
 #endif /* __mc68000__ */
 	if (match)
-		printk(" (%sed with %s)",
+		printk(KERN_CONT " (%sed with %s)",
 			hwif->sharing_irq ? "shar" : "serializ", match->name);
-	printk("\n");
+	printk(KERN_CONT "\n");
 
 	mutex_unlock(&ide_cfg_mtx);
 	return 0;
@@ -1287,7 +1280,7 @@ static int hwif_init(ide_hwif_t *hwif)
 	if (!hwif->irq) {
 		hwif->irq = __ide_default_irq(hwif->io_ports.data_addr);
 		if (!hwif->irq) {
-			printk("%s: DISABLED, NO IRQ\n", hwif->name);
+			printk(KERN_ERR "%s: disabled, no IRQ\n", hwif->name);
 			return 0;
 		}
 	}
@@ -1317,16 +1310,16 @@ static int hwif_init(ide_hwif_t *hwif)
 	 */
 	hwif->irq = __ide_default_irq(hwif->io_ports.data_addr);
 	if (!hwif->irq) {
-		printk("%s: Disabled unable to get IRQ %d.\n",
+		printk(KERN_ERR "%s: disabled, unable to get IRQ %d\n",
 			hwif->name, old_irq);
 		goto out;
 	}
 	if (init_irq(hwif)) {
-		printk("%s: probed IRQ %d and default IRQ %d failed.\n",
+		printk(KERN_ERR "%s: probed IRQ %d and default IRQ %d failed\n",
 			hwif->name, old_irq, hwif->irq);
 		goto out;
 	}
-	printk("%s: probed IRQ %d failed, using default.\n",
+	printk(KERN_WARNING "%s: probed IRQ %d failed, using default\n",
 		hwif->name, hwif->irq);
 
 done:
@@ -1595,6 +1588,8 @@ struct ide_host *ide_host_alloc_all(const struct ide_port_info *d,
 
 		ide_init_port_data(hwif, idx);
 
+		hwif->host = host;
+
 		host->ports[i] = hwif;
 		host->n_ports++;
 	}
@@ -1604,6 +1599,12 @@ struct ide_host *ide_host_alloc_all(const struct ide_port_info *d,
 		return NULL;
 	}
 
+	if (hws[0])
+		host->dev[0] = hws[0]->dev;
+
+	if (d)
+		host->host_flags = d->host_flags;
+
 	return host;
 }
 EXPORT_SYMBOL_GPL(ide_host_alloc_all);
diff --git a/drivers/ide/ide-proc.c b/drivers/ide/ide-proc.c
index 151c91e933d..f66c9c3f6fc 100644
--- a/drivers/ide/ide-proc.c
+++ b/drivers/ide/ide-proc.c
@@ -105,7 +105,7 @@ static int proc_ide_read_identify
 	len = sprintf(page, "\n");
 
 	if (drive) {
-		unsigned short *val = (unsigned short *) page;
+		__le16 *val = (__le16 *)page;
 
 		err = taskfile_lib_get_identify(drive, page);
 		if (!err) {
@@ -113,7 +113,7 @@ static int proc_ide_read_identify
 			page = out;
 			do {
 				out += sprintf(out, "%04x%c",
-					le16_to_cpu(*val), (++i & 7) ? ' ' : '\n');
+					le16_to_cpup(val), (++i & 7) ? ' ' : '\n');
 				val += 1;
 			} while (i < (SECTOR_WORDS * 2));
 			len = out - page;
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index 6962ca4891a..82c2afe4d28 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -322,23 +322,29 @@ static struct class *idetape_sysfs_class;
 #define ide_tape_g(disk) \
 	container_of((disk)->private_data, struct ide_tape_obj, driver)
 
+static void ide_tape_release(struct kref *);
+
 static struct ide_tape_obj *ide_tape_get(struct gendisk *disk)
 {
 	struct ide_tape_obj *tape = NULL;
 
 	mutex_lock(&idetape_ref_mutex);
 	tape = ide_tape_g(disk);
-	if (tape)
+	if (tape) {
 		kref_get(&tape->kref);
+		if (ide_device_get(tape->drive)) {
+			kref_put(&tape->kref, ide_tape_release);
+			tape = NULL;
+		}
+	}
 	mutex_unlock(&idetape_ref_mutex);
 	return tape;
 }
 
-static void ide_tape_release(struct kref *);
-
 static void ide_tape_put(struct ide_tape_obj *tape)
 {
 	mutex_lock(&idetape_ref_mutex);
+	ide_device_put(tape->drive);
 	kref_put(&tape->kref, ide_tape_release);
 	mutex_unlock(&idetape_ref_mutex);
 }
@@ -649,10 +655,10 @@ static void ide_tape_callback(ide_drive_t *drive)
 			uptodate = 0;
 		} else {
 			debug_log(DBG_SENSE, "Block Location - %u\n",
-					be32_to_cpu(*(u32 *)&readpos[4]));
+					be32_to_cpup((__be32 *)&readpos[4]));
 
 			tape->partition = readpos[1];
-			tape->first_frame = be32_to_cpu(*(u32 *)&readpos[4]);
+			tape->first_frame = be32_to_cpup((__be32 *)&readpos[4]);
 			set_bit(IDE_AFLAG_ADDRESS_VALID, &drive->atapi_flags);
 		}
 	}
@@ -2375,23 +2381,23 @@ static void idetape_get_mode_sense_results(ide_drive_t *drive)
 	caps = pc.buf + 4 + pc.buf[3];
 
 	/* convert to host order and save for later use */
-	speed = be16_to_cpu(*(u16 *)&caps[14]);
-	max_speed = be16_to_cpu(*(u16 *)&caps[8]);
+	speed = be16_to_cpup((__be16 *)&caps[14]);
+	max_speed = be16_to_cpup((__be16 *)&caps[8]);
 
-	put_unaligned(max_speed, (u16 *)&caps[8]);
-	put_unaligned(be16_to_cpu(*(u16 *)&caps[12]), (u16 *)&caps[12]);
-	put_unaligned(speed, (u16 *)&caps[14]);
-	put_unaligned(be16_to_cpu(*(u16 *)&caps[16]), (u16 *)&caps[16]);
+	*(u16 *)&caps[8] = max_speed;
+	*(u16 *)&caps[12] = be16_to_cpup((__be16 *)&caps[12]);
+	*(u16 *)&caps[14] = speed;
+	*(u16 *)&caps[16] = be16_to_cpup((__be16 *)&caps[16]);
 
 	if (!speed) {
 		printk(KERN_INFO "ide-tape: %s: invalid tape speed "
 				"(assuming 650KB/sec)\n", drive->name);
-		put_unaligned(650, (u16 *)&caps[14]);
+		*(u16 *)&caps[14] = 650;
 	}
 	if (!max_speed) {
 		printk(KERN_INFO "ide-tape: %s: invalid max_speed "
 				"(assuming 650KB/sec)\n", drive->name);
-		put_unaligned(650, (u16 *)&caps[8]);
+		*(u16 *)&caps[8] = 650;
 	}
 
 	memcpy(&tape->caps, caps, 20);
diff --git a/drivers/ide/ide-taskfile.c b/drivers/ide/ide-taskfile.c
index aeddbbd69e8..7fb6f1c8627 100644
--- a/drivers/ide/ide-taskfile.c
+++ b/drivers/ide/ide-taskfile.c
@@ -126,7 +126,10 @@ EXPORT_SYMBOL_GPL(do_rw_taskfile);
 static ide_startstop_t set_multmode_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 stat = hwif->tp_ops->read_status(hwif);
+	u8 stat;
+
+	local_irq_enable_in_hardirq();
+	stat = hwif->tp_ops->read_status(hwif);
 
 	if (OK_STAT(stat, READY_STAT, BAD_STAT))
 		drive->mult_count = drive->mult_req;
@@ -147,6 +150,8 @@ static ide_startstop_t set_geometry_intr(ide_drive_t *drive)
 	int retries = 5;
 	u8 stat;
 
+	local_irq_enable_in_hardirq();
+
 	while (1) {
 		stat = hwif->tp_ops->read_status(hwif);
 		if ((stat & BUSY_STAT) == 0 || retries-- == 0)
@@ -170,7 +175,10 @@ static ide_startstop_t set_geometry_intr(ide_drive_t *drive)
 static ide_startstop_t recal_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 stat = hwif->tp_ops->read_status(hwif);
+	u8 stat;
+
+	local_irq_enable_in_hardirq();
+	stat = hwif->tp_ops->read_status(hwif);
 
 	if (!OK_STAT(stat, READY_STAT, BAD_STAT))
 		return ide_error(drive, "recal_intr", stat);
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index 60f0ca66aa9..772451600e4 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -618,6 +618,53 @@ set_val:
 
 EXPORT_SYMBOL(generic_ide_ioctl);
 
+/**
+ * ide_device_get	-	get an additional reference to a ide_drive_t
+ * @drive:	device to get a reference to
+ *
+ * Gets a reference to the ide_drive_t and increments the use count of the
+ * underlying LLDD module.
+ */
+int ide_device_get(ide_drive_t *drive)
+{
+	struct device *host_dev;
+	struct module *module;
+
+	if (!get_device(&drive->gendev))
+		return -ENXIO;
+
+	host_dev = drive->hwif->host->dev[0];
+	module = host_dev ? host_dev->driver->owner : NULL;
+
+	if (module && !try_module_get(module)) {
+		put_device(&drive->gendev);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ide_device_get);
+
+/**
+ * ide_device_put	-	release a reference to a ide_drive_t
+ * @drive:	device to release a reference on
+ *
+ * Release a reference to the ide_drive_t and decrements the use count of
+ * the underlying LLDD module.
+ */
+void ide_device_put(ide_drive_t *drive)
+{
+#ifdef CONFIG_MODULE_UNLOAD
+	struct device *host_dev = drive->hwif->host->dev[0];
+	struct module *module = host_dev ? host_dev->driver->owner : NULL;
+
+	if (module)
+		module_put(module);
+#endif
+	put_device(&drive->gendev);
+}
+EXPORT_SYMBOL_GPL(ide_device_put);
+
 static int ide_bus_match(struct device *dev, struct device_driver *drv)
 {
 	return 1;
diff --git a/drivers/ide/legacy/gayle.c b/drivers/ide/legacy/gayle.c
index dd5c467d8dd..51ba085d7aa 100644
--- a/drivers/ide/legacy/gayle.c
+++ b/drivers/ide/legacy/gayle.c
@@ -127,7 +127,7 @@ static int __init gayle_init(void)
     unsigned long phys_base, res_start, res_n;
     unsigned long base, ctrlport, irqport;
     ide_ack_intr_t *ack_intr;
-    int a4000, i;
+    int a4000, i, rc;
     hw_regs_t hw[GAYLE_NUM_HWIFS], *hws[] = { NULL, NULL, NULL, NULL };
 
     if (!MACH_IS_AMIGA)
@@ -179,7 +179,11 @@ found:
 	hws[i] = &hw[i];
     }
 
-    return ide_host_add(NULL, hws, NULL);
+    rc = ide_host_add(NULL, hws, NULL);
+    if (rc)
+	release_mem_region(res_start, res_n);
+
+    return rc;
 }
 
 module_init(gayle_init);
diff --git a/drivers/ide/legacy/ht6560b.c b/drivers/ide/legacy/ht6560b.c
index 7bc8fd59ea9..98f7c95e39e 100644
--- a/drivers/ide/legacy/ht6560b.c
+++ b/drivers/ide/legacy/ht6560b.c
@@ -3,34 +3,12 @@
  */
 
 /*
- *
- *  Version 0.01        Initial version hacked out of ide.c
- *
- *  Version 0.02        Added support for PIO modes, auto-tune
- *
- *  Version 0.03        Some cleanups
- *
- *  Version 0.05        PIO mode cycle timings auto-tune using bus-speed
- *
- *  Version 0.06        Prefetch mode now defaults no OFF. To set
- *                      prefetch mode OFF/ON use "hdparm -p8/-p9".
- *                      Unmask irq is disabled when prefetch mode
- *                      is enabled.
- *
- *  Version 0.07        Trying to fix CD-ROM detection problem.
- *                      "Prefetch" mode bit OFF for ide disks and
- *                      ON for anything else.
- *
- *  Version 0.08        Need to force prefetch for CDs and other non-disk
- *                      devices. (not sure which devices exactly need
- *                      prefetch)
- *
  *  HT-6560B EIDE-controller support
  *  To activate controller support use kernel parameter "ide0=ht6560b".
  *  Use hdparm utility to enable PIO mode support.
  *
  *  Author:    Mikko Ala-Fossi            <maf@iki.fi>
- *             Jan Evert van Grootheest   <janevert@caiway.nl>
+ *             Jan Evert van Grootheest   <j.e.van.grootheest@caiway.nl>
  *
  *  Try:  http://www.maf.iki.fi/~maf/ht6560b/
  */
diff --git a/drivers/ide/pci/aec62xx.c b/drivers/ide/pci/aec62xx.c
index fbc43e121e6..e0c8fe7d9fe 100644
--- a/drivers/ide/pci/aec62xx.c
+++ b/drivers/ide/pci/aec62xx.c
@@ -13,6 +13,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "aec62xx"
+
 struct chipset_bus_clock_list_entry {
 	u8 xfer_speed;
 	u8 chipset_settings;
@@ -59,10 +61,6 @@ static const struct chipset_bus_clock_list_entry aec6xxx_34_base [] = {
 	{	0,		0x00,	0x00	}
 };
 
-#define BUSCLOCK(D)	\
-	((struct chipset_bus_clock_list_entry *) pci_get_drvdata((D)))
-
-
 /*
  * TO DO: active tuning and correction of cards without a bios.
  */
@@ -88,6 +86,8 @@ static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
 	u16 d_conf		= 0;
 	u8 ultra = 0, ultra_conf = 0;
 	u8 tmp0 = 0, tmp1 = 0, tmp2 = 0;
@@ -96,7 +96,7 @@ static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 	local_irq_save(flags);
 	/* 0x40|(2*drive->dn): Active, 0x41|(2*drive->dn): Recovery */
 	pci_read_config_word(dev, 0x40|(2*drive->dn), &d_conf);
-	tmp0 = pci_bus_clock_list(speed, BUSCLOCK(dev));
+	tmp0 = pci_bus_clock_list(speed, bus_clock);
 	d_conf = ((tmp0 & 0xf0) << 4) | (tmp0 & 0xf);
 	pci_write_config_word(dev, 0x40|(2*drive->dn), d_conf);
 
@@ -104,7 +104,7 @@ static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 	tmp2 = 0x00;
 	pci_read_config_byte(dev, 0x54, &ultra);
 	tmp1 = ((0x00 << (2*drive->dn)) | (ultra & ~(3 << (2*drive->dn))));
-	ultra_conf = pci_bus_clock_list_ultra(speed, BUSCLOCK(dev));
+	ultra_conf = pci_bus_clock_list_ultra(speed, bus_clock);
 	tmp2 = ((ultra_conf << (2*drive->dn)) | (tmp1 & ~(3 << (2*drive->dn))));
 	pci_write_config_byte(dev, 0x54, tmp2);
 	local_irq_restore(flags);
@@ -114,6 +114,8 @@ static void aec6260_set_mode(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
 	u8 unit		= (drive->select.b.unit & 0x01);
 	u8 tmp1 = 0, tmp2 = 0;
 	u8 ultra = 0, drive_conf = 0, ultra_conf = 0;
@@ -122,12 +124,12 @@ static void aec6260_set_mode(ide_drive_t *drive, const u8 speed)
 	local_irq_save(flags);
 	/* high 4-bits: Active, low 4-bits: Recovery */
 	pci_read_config_byte(dev, 0x40|drive->dn, &drive_conf);
-	drive_conf = pci_bus_clock_list(speed, BUSCLOCK(dev));
+	drive_conf = pci_bus_clock_list(speed, bus_clock);
 	pci_write_config_byte(dev, 0x40|drive->dn, drive_conf);
 
 	pci_read_config_byte(dev, (0x44|hwif->channel), &ultra);
 	tmp1 = ((0x00 << (4*unit)) | (ultra & ~(7 << (4*unit))));
-	ultra_conf = pci_bus_clock_list_ultra(speed, BUSCLOCK(dev));
+	ultra_conf = pci_bus_clock_list_ultra(speed, bus_clock);
 	tmp2 = ((ultra_conf << (4*unit)) | (tmp1 & ~(7 << (4*unit))));
 	pci_write_config_byte(dev, (0x44|hwif->channel), tmp2);
 	local_irq_restore(flags);
@@ -138,15 +140,8 @@ static void aec_set_pio_mode(ide_drive_t *drive, const u8 pio)
 	drive->hwif->port_ops->set_dma_mode(drive, pio + XFER_PIO_0);
 }
 
-static unsigned int __devinit init_chipset_aec62xx(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_aec62xx(struct pci_dev *dev)
 {
-	int bus_speed = ide_pci_clk ? ide_pci_clk : 33;
-
-	if (bus_speed <= 33)
-		pci_set_drvdata(dev, (void *) aec6xxx_33_base);
-	else
-		pci_set_drvdata(dev, (void *) aec6xxx_34_base);
-
 	/* These are necessary to get AEC6280 Macintosh cards to work */
 	if ((dev->device == PCI_DEVICE_ID_ARTOP_ATP865) ||
 	    (dev->device == PCI_DEVICE_ID_ARTOP_ATP865R)) {
@@ -187,8 +182,8 @@ static const struct ide_port_ops atp86x_port_ops = {
 };
 
 static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
-	{	/* 0 */
-		.name		= "AEC6210",
+	{	/* 0: AEC6210 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_aec62xx,
 		.enablebits	= {{0x4a,0x02,0x02}, {0x4a,0x04,0x04}},
 		.port_ops	= &atp850_port_ops,
@@ -199,8 +194,9 @@ static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA2,
-	},{	/* 1 */
-		.name		= "AEC6260",
+	},
+	{	/* 1: AEC6260 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_aec62xx,
 		.port_ops	= &atp86x_port_ops,
 		.host_flags	= IDE_HFLAG_NO_ATAPI_DMA | IDE_HFLAG_NO_AUTODMA |
@@ -208,8 +204,9 @@ static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA4,
-	},{	/* 2 */
-		.name		= "AEC6260R",
+	},
+	{	/* 2: AEC6260R */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_aec62xx,
 		.enablebits	= {{0x4a,0x02,0x02}, {0x4a,0x04,0x04}},
 		.port_ops	= &atp86x_port_ops,
@@ -218,8 +215,9 @@ static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA4,
-	},{	/* 3 */
-		.name		= "AEC6280",
+	},
+	{	/* 3: AEC6280 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_aec62xx,
 		.port_ops	= &atp86x_port_ops,
 		.host_flags	= IDE_HFLAG_NO_ATAPI_DMA |
@@ -227,8 +225,9 @@ static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
-	},{	/* 4 */
-		.name		= "AEC6280R",
+	},
+	{	/* 4: AEC6280R */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_aec62xx,
 		.enablebits	= {{0x4a,0x02,0x02}, {0x4a,0x04,0x04}},
 		.port_ops	= &atp86x_port_ops,
@@ -254,10 +253,17 @@ static const struct ide_port_info aec62xx_chipsets[] __devinitdata = {
 
 static int __devinit aec62xx_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
+	const struct chipset_bus_clock_list_entry *bus_clock;
 	struct ide_port_info d;
 	u8 idx = id->driver_data;
+	int bus_speed = ide_pci_clk ? ide_pci_clk : 33;
 	int err;
 
+	if (bus_speed <= 33)
+		bus_clock = aec6xxx_33_base;
+	else
+		bus_clock = aec6xxx_34_base;
+
 	err = pci_enable_device(dev);
 	if (err)
 		return err;
@@ -268,18 +274,25 @@ static int __devinit aec62xx_init_one(struct pci_dev *dev, const struct pci_devi
 		unsigned long dma_base = pci_resource_start(dev, 4);
 
 		if (inb(dma_base + 2) & 0x10) {
-			d.name = (idx == 4) ? "AEC6880R" : "AEC6880";
+			printk(KERN_INFO DRV_NAME " %s: AEC6880%s card detected"
+				"\n", pci_name(dev), (idx == 4) ? "R" : "");
 			d.udma_mask = ATA_UDMA6;
 		}
 	}
 
-	err = ide_setup_pci_device(dev, &d);
+	err = ide_pci_init_one(dev, &d, (void *)bus_clock);
 	if (err)
 		pci_disable_device(dev);
 
 	return err;
 }
 
+static void __devexit aec62xx_remove(struct pci_dev *dev)
+{
+	ide_pci_remove(dev);
+	pci_disable_device(dev);
+}
+
 static const struct pci_device_id aec62xx_pci_tbl[] = {
 	{ PCI_VDEVICE(ARTOP, PCI_DEVICE_ID_ARTOP_ATP850UF), 0 },
 	{ PCI_VDEVICE(ARTOP, PCI_DEVICE_ID_ARTOP_ATP860),   1 },
@@ -294,6 +307,7 @@ static struct pci_driver driver = {
 	.name		= "AEC62xx_IDE",
 	.id_table	= aec62xx_pci_tbl,
 	.probe		= aec62xx_init_one,
+	.remove		= aec62xx_remove,
 };
 
 static int __init aec62xx_ide_init(void)
@@ -301,7 +315,13 @@ static int __init aec62xx_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit aec62xx_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(aec62xx_ide_init);
+module_exit(aec62xx_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for ARTOP AEC62xx IDE");
diff --git a/drivers/ide/pci/alim15x3.c b/drivers/ide/pci/alim15x3.c
index 5ef7817ac64..b582687e0cd 100644
--- a/drivers/ide/pci/alim15x3.c
+++ b/drivers/ide/pci/alim15x3.c
@@ -38,6 +38,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "alim15x3"
+
 /*
  * Allow UDMA on M1543C-E chipset for WDC disks that ignore CRC checking
  * (this is DANGEROUS and could result in data corruption).
@@ -207,13 +209,12 @@ static int ali15x3_dma_setup(ide_drive_t *drive)
 /**
  *	init_chipset_ali15x3	-	Initialise an ALi IDE controller
  *	@dev: PCI device
- *	@name: Name of the controller
  *
  *	This function initializes the ALI IDE controller and where 
  *	appropriate also sets up the 1533 southbridge.
  */
-  
-static unsigned int __devinit init_chipset_ali15x3 (struct pci_dev *dev, const char *name)
+
+static unsigned int __devinit init_chipset_ali15x3(struct pci_dev *dev)
 {
 	unsigned long flags;
 	u8 tmpbyte;
@@ -515,7 +516,7 @@ static const struct ide_dma_ops ali_dma_ops = {
 };
 
 static const struct ide_port_info ali15x3_chipset __devinitdata = {
-	.name		= "ALI15X3",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_ali15x3,
 	.init_hwif	= init_hwif_ali15x3,
 	.init_dma	= init_dma_ali15x3,
@@ -565,7 +566,7 @@ static int __devinit alim15x3_init_one(struct pci_dev *dev, const struct pci_dev
 	if (idx == 0)
 		d.host_flags |= IDE_HFLAG_CLEAR_SIMPLEX;
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 
@@ -580,6 +581,7 @@ static struct pci_driver driver = {
 	.name		= "ALI15x3_IDE",
 	.id_table	= alim15x3_pci_tbl,
 	.probe		= alim15x3_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init ali15x3_ide_init(void)
@@ -587,7 +589,13 @@ static int __init ali15x3_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit ali15x3_ide_exit(void)
+{
+	return pci_unregister_driver(&driver);
+}
+
 module_init(ali15x3_ide_init);
+module_exit(ali15x3_ide_exit);
 
 MODULE_AUTHOR("Michael Aubry, Andrzej Krzysztofowicz, CJ, Andre Hedrick, Alan Cox");
 MODULE_DESCRIPTION("PCI driver module for ALi 15x3 IDE");
diff --git a/drivers/ide/pci/amd74xx.c b/drivers/ide/pci/amd74xx.c
index ef7d971031e..2cea7bf51a0 100644
--- a/drivers/ide/pci/amd74xx.c
+++ b/drivers/ide/pci/amd74xx.c
@@ -21,6 +21,8 @@
 #include <linux/init.h>
 #include <linux/ide.h>
 
+#define DRV_NAME "amd74xx"
+
 enum {
 	AMD_IDE_CONFIG		= 0x41,
 	AMD_CABLE_DETECT	= 0x42,
@@ -110,15 +112,13 @@ static void amd_set_pio_mode(ide_drive_t *drive, const u8 pio)
 	amd_set_drive(drive, XFER_PIO_0 + pio);
 }
 
-static void __devinit amd7409_cable_detect(struct pci_dev *dev,
-					   const char *name)
+static void __devinit amd7409_cable_detect(struct pci_dev *dev)
 {
 	/* no host side cable detection */
 	amd_80w = 0x03;
 }
 
-static void __devinit amd7411_cable_detect(struct pci_dev *dev,
-					   const char *name)
+static void __devinit amd7411_cable_detect(struct pci_dev *dev)
 {
 	int i;
 	u32 u = 0;
@@ -129,9 +129,9 @@ static void __devinit amd7411_cable_detect(struct pci_dev *dev,
 	amd_80w = ((t & 0x3) ? 1 : 0) | ((t & 0xc) ? 2 : 0);
 	for (i = 24; i >= 0; i -= 8)
 		if (((u >> i) & 4) && !(amd_80w & (1 << (1 - (i >> 4))))) {
-			printk(KERN_WARNING "%s: BIOS didn't set cable bits "
-					    "correctly. Enabling workaround.\n",
-					    name);
+			printk(KERN_WARNING DRV_NAME " %s: BIOS didn't set "
+				"cable bits correctly. Enabling workaround.\n",
+				pci_name(dev));
 			amd_80w |= (1 << (1 - (i >> 4)));
 		}
 }
@@ -140,8 +140,7 @@ static void __devinit amd7411_cable_detect(struct pci_dev *dev,
  * The initialization callback.  Initialize drive independent registers.
  */
 
-static unsigned int __devinit init_chipset_amd74xx(struct pci_dev *dev,
-						   const char *name)
+static unsigned int __devinit init_chipset_amd74xx(struct pci_dev *dev)
 {
 	u8 t = 0, offset = amd_offset(dev);
 
@@ -154,9 +153,9 @@ static unsigned int __devinit init_chipset_amd74xx(struct pci_dev *dev,
 		; /* no UDMA > 2 */
 	else if (dev->vendor == PCI_VENDOR_ID_AMD &&
 		 dev->device == PCI_DEVICE_ID_AMD_VIPER_7409)
-		amd7409_cable_detect(dev, name);
+		amd7409_cable_detect(dev);
 	else
-		amd7411_cable_detect(dev, name);
+		amd7411_cable_detect(dev);
 
 /*
  * Take care of prefetch & postwrite.
@@ -173,24 +172,6 @@ static unsigned int __devinit init_chipset_amd74xx(struct pci_dev *dev,
 		t |= 0xf0;
 	pci_write_config_byte(dev, AMD_IDE_CONFIG + offset, t);
 
-/*
- * Determine the system bus clock.
- */
-
-	amd_clock = (ide_pci_clk ? ide_pci_clk : 33) * 1000;
-
-	switch (amd_clock) {
-		case 33000: amd_clock = 33333; break;
-		case 37000: amd_clock = 37500; break;
-		case 41000: amd_clock = 41666; break;
-	}
-
-	if (amd_clock < 20000 || amd_clock > 50000) {
-		printk(KERN_WARNING "%s: User given PCI clock speed impossible (%d), using 33 MHz instead.\n",
-				    name, amd_clock);
-		amd_clock = 33333;
-	}
-
 	return dev->irq;
 }
 
@@ -222,9 +203,9 @@ static const struct ide_port_ops amd_port_ops = {
 	 IDE_HFLAG_IO_32BIT | \
 	 IDE_HFLAG_UNMASK_IRQS)
 
-#define DECLARE_AMD_DEV(name_str, swdma, udma)				\
+#define DECLARE_AMD_DEV(swdma, udma)				\
 	{								\
-		.name		= name_str,				\
+		.name		= DRV_NAME,				\
 		.init_chipset	= init_chipset_amd74xx,			\
 		.init_hwif	= init_hwif_amd74xx,			\
 		.enablebits	= {{0x40,0x02,0x02}, {0x40,0x01,0x01}},	\
@@ -236,9 +217,9 @@ static const struct ide_port_ops amd_port_ops = {
 		.udma_mask	= udma,					\
 	}
 
-#define DECLARE_NV_DEV(name_str, udma)					\
+#define DECLARE_NV_DEV(udma)					\
 	{								\
-		.name		= name_str,				\
+		.name		= DRV_NAME,				\
 		.init_chipset	= init_chipset_amd74xx,			\
 		.init_hwif	= init_hwif_amd74xx,			\
 		.enablebits	= {{0x50,0x02,0x02}, {0x50,0x01,0x01}},	\
@@ -251,31 +232,15 @@ static const struct ide_port_ops amd_port_ops = {
 	}
 
 static const struct ide_port_info amd74xx_chipsets[] __devinitdata = {
-	/*  0 */ DECLARE_AMD_DEV("AMD7401",	  0x00, ATA_UDMA2),
-	/*  1 */ DECLARE_AMD_DEV("AMD7409", ATA_SWDMA2, ATA_UDMA4),
-	/*  2 */ DECLARE_AMD_DEV("AMD7411", ATA_SWDMA2, ATA_UDMA5),
-	/*  3 */ DECLARE_AMD_DEV("AMD7441", ATA_SWDMA2, ATA_UDMA5),
-	/*  4 */ DECLARE_AMD_DEV("AMD8111", ATA_SWDMA2, ATA_UDMA6),
-
-	/*  5 */ DECLARE_NV_DEV("NFORCE",		ATA_UDMA5),
-	/*  6 */ DECLARE_NV_DEV("NFORCE2",		ATA_UDMA6),
-	/*  7 */ DECLARE_NV_DEV("NFORCE2-U400R",	ATA_UDMA6),
-	/*  8 */ DECLARE_NV_DEV("NFORCE2-U400R-SATA",	ATA_UDMA6),
-	/*  9 */ DECLARE_NV_DEV("NFORCE3-150",		ATA_UDMA6),
-	/* 10 */ DECLARE_NV_DEV("NFORCE3-250",		ATA_UDMA6),
-	/* 11 */ DECLARE_NV_DEV("NFORCE3-250-SATA",	ATA_UDMA6),
-	/* 12 */ DECLARE_NV_DEV("NFORCE3-250-SATA2",	ATA_UDMA6),
-	/* 13 */ DECLARE_NV_DEV("NFORCE-CK804",		ATA_UDMA6),
-	/* 14 */ DECLARE_NV_DEV("NFORCE-MCP04",		ATA_UDMA6),
-	/* 15 */ DECLARE_NV_DEV("NFORCE-MCP51",		ATA_UDMA6),
-	/* 16 */ DECLARE_NV_DEV("NFORCE-MCP55",		ATA_UDMA6),
-	/* 17 */ DECLARE_NV_DEV("NFORCE-MCP61",		ATA_UDMA6),
-	/* 18 */ DECLARE_NV_DEV("NFORCE-MCP65",		ATA_UDMA6),
-	/* 19 */ DECLARE_NV_DEV("NFORCE-MCP67",		ATA_UDMA6),
-	/* 20 */ DECLARE_NV_DEV("NFORCE-MCP73",		ATA_UDMA6),
-	/* 21 */ DECLARE_NV_DEV("NFORCE-MCP77",		ATA_UDMA6),
-
-	/* 22 */ DECLARE_AMD_DEV("AMD5536", ATA_SWDMA2, ATA_UDMA5),
+	/* 0: AMD7401 */	DECLARE_AMD_DEV(0x00, ATA_UDMA2),
+	/* 1: AMD7409 */	DECLARE_AMD_DEV(ATA_SWDMA2, ATA_UDMA4),
+	/* 2: AMD7411/7441 */	DECLARE_AMD_DEV(ATA_SWDMA2, ATA_UDMA5),
+	/* 3: AMD8111 */	DECLARE_AMD_DEV(ATA_SWDMA2, ATA_UDMA6),
+
+	/* 4: NFORCE */		DECLARE_NV_DEV(ATA_UDMA5),
+	/* 5: >= NFORCE2 */	DECLARE_NV_DEV(ATA_UDMA6),
+
+	/* 6: AMD5536 */	DECLARE_AMD_DEV(ATA_SWDMA2, ATA_UDMA5),
 };
 
 static int __devinit amd74xx_probe(struct pci_dev *dev, const struct pci_device_id *id)
@@ -292,47 +257,64 @@ static int __devinit amd74xx_probe(struct pci_dev *dev, const struct pci_device_
 		if (dev->revision <= 7)
 			d.swdma_mask = 0;
 		d.host_flags |= IDE_HFLAG_CLEAR_SIMPLEX;
-	} else if (idx == 4) {
+	} else if (idx == 3) {
 		if (dev->subsystem_vendor == PCI_VENDOR_ID_AMD &&
 		    dev->subsystem_device == PCI_DEVICE_ID_AMD_SERENADE)
 			d.udma_mask = ATA_UDMA5;
 	}
 
-	printk(KERN_INFO "%s: %s (rev %02x) UDMA%s controller\n",
-			 d.name, pci_name(dev), dev->revision,
-			 amd_dma[fls(d.udma_mask) - 1]);
+	printk(KERN_INFO "%s %s: UDMA%s controller\n",
+		d.name, pci_name(dev), amd_dma[fls(d.udma_mask) - 1]);
+
+	/*
+	* Determine the system bus clock.
+	*/
+	amd_clock = (ide_pci_clk ? ide_pci_clk : 33) * 1000;
+
+	switch (amd_clock) {
+	case 33000: amd_clock = 33333; break;
+	case 37000: amd_clock = 37500; break;
+	case 41000: amd_clock = 41666; break;
+	}
+
+	if (amd_clock < 20000 || amd_clock > 50000) {
+		printk(KERN_WARNING "%s: User given PCI clock speed impossible"
+				    " (%d), using 33 MHz instead.\n",
+				    d.name, amd_clock);
+		amd_clock = 33333;
+	}
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 static const struct pci_device_id amd74xx_pci_tbl[] = {
 	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_COBRA_7401),		 0 },
 	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_VIPER_7409),		 1 },
 	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_VIPER_7411),		 2 },
-	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_OPUS_7441),		 3 },
-	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_8111_IDE),		 4 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_IDE),	 5 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2_IDE),	 6 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2S_IDE),	 7 },
+	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_OPUS_7441),		 2 },
+	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_8111_IDE),		 3 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_IDE),	 4 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2S_IDE),	 5 },
 #ifdef CONFIG_BLK_DEV_IDE_SATA
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2S_SATA),	 8 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE2S_SATA),	 5 },
 #endif
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3_IDE),	 9 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_IDE),	10 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_IDE),	 5 },
 #ifdef CONFIG_BLK_DEV_IDE_SATA
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_SATA),	11 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_SATA2),	12 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_SATA),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE3S_SATA2),	 5 },
 #endif
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_CK804_IDE),	13 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP04_IDE),	14 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_IDE),	15 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_IDE),	16 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_IDE),	17 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP65_IDE),	18 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP67_IDE),	19 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP73_IDE),	20 },
-	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP77_IDE),	21 },
-	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_CS5536_IDE),		22 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_CK804_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP04_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP65_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP67_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP73_IDE),	 5 },
+	{ PCI_VDEVICE(NVIDIA,	PCI_DEVICE_ID_NVIDIA_NFORCE_MCP77_IDE),	 5 },
+	{ PCI_VDEVICE(AMD,	PCI_DEVICE_ID_AMD_CS5536_IDE),		 6 },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, amd74xx_pci_tbl);
@@ -341,6 +323,7 @@ static struct pci_driver driver = {
 	.name		= "AMD_IDE",
 	.id_table	= amd74xx_pci_tbl,
 	.probe		= amd74xx_probe,
+	.remove		= ide_pci_remove,
 };
 
 static int __init amd74xx_ide_init(void)
@@ -348,7 +331,13 @@ static int __init amd74xx_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit amd74xx_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(amd74xx_ide_init);
+module_exit(amd74xx_ide_exit);
 
 MODULE_AUTHOR("Vojtech Pavlik");
 MODULE_DESCRIPTION("AMD PCI IDE driver");
diff --git a/drivers/ide/pci/atiixp.c b/drivers/ide/pci/atiixp.c
index 8b637181681..332f08f43b5 100644
--- a/drivers/ide/pci/atiixp.c
+++ b/drivers/ide/pci/atiixp.c
@@ -11,6 +11,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "atiixp"
+
 #define ATIIXP_IDE_PIO_TIMING		0x40
 #define ATIIXP_IDE_MDMA_TIMING		0x44
 #define ATIIXP_IDE_PIO_CONTROL		0x48
@@ -137,16 +139,17 @@ static const struct ide_port_ops atiixp_port_ops = {
 };
 
 static const struct ide_port_info atiixp_pci_info[] __devinitdata = {
-	{	/* 0 */
-		.name		= "ATIIXP",
+	{	/* 0: IXP200/300/400/700 */
+		.name		= DRV_NAME,
 		.enablebits	= {{0x48,0x01,0x00}, {0x48,0x08,0x00}},
 		.port_ops	= &atiixp_port_ops,
 		.host_flags	= IDE_HFLAG_LEGACY_IRQS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
-	},{	/* 1 */
-		.name		= "SB600_PATA",
+	},
+	{	/* 1: IXP600 */
+		.name		= DRV_NAME,
 		.enablebits	= {{0x48,0x01,0x00}, {0x00,0x00,0x00}},
 		.port_ops	= &atiixp_port_ops,
 		.host_flags	= IDE_HFLAG_SINGLE | IDE_HFLAG_LEGACY_IRQS,
@@ -167,7 +170,7 @@ static const struct ide_port_info atiixp_pci_info[] __devinitdata = {
 
 static int __devinit atiixp_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &atiixp_pci_info[id->driver_data]);
+	return ide_pci_init_one(dev, &atiixp_pci_info[id->driver_data], NULL);
 }
 
 static const struct pci_device_id atiixp_pci_tbl[] = {
@@ -184,6 +187,7 @@ static struct pci_driver driver = {
 	.name		= "ATIIXP_IDE",
 	.id_table	= atiixp_pci_tbl,
 	.probe		= atiixp_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init atiixp_ide_init(void)
@@ -191,7 +195,13 @@ static int __init atiixp_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit atiixp_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(atiixp_ide_init);
+module_exit(atiixp_ide_exit);
 
 MODULE_AUTHOR("HUI YU");
 MODULE_DESCRIPTION("PCI driver module for ATI IXP IDE");
diff --git a/drivers/ide/pci/cmd64x.c b/drivers/ide/pci/cmd64x.c
index ce58bfcdb3c..1360b4fa9fd 100644
--- a/drivers/ide/pci/cmd64x.c
+++ b/drivers/ide/pci/cmd64x.c
@@ -19,6 +19,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "cmd64x"
+
 #define CMD_DEBUG 0
 
 #if CMD_DEBUG
@@ -330,28 +332,10 @@ static int cmd646_1_dma_end(ide_drive_t *drive)
 	return (dma_stat & 7) != 4;
 }
 
-static unsigned int __devinit init_chipset_cmd64x(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_cmd64x(struct pci_dev *dev)
 {
 	u8 mrdmode = 0;
 
-	if (dev->device == PCI_DEVICE_ID_CMD_646) {
-
-		switch (dev->revision) {
-		case 0x07:
-		case 0x05:
-			printk("%s: UltraDMA capable\n", name);
-			break;
-		case 0x03:
-		default:
-			printk("%s: MultiWord DMA force limited\n", name);
-			break;
-		case 0x01:
-			printk("%s: MultiWord DMA limited, "
-			       "IRQ workaround enabled\n", name);
-			break;
-		}
-	}
-
 	/* Set a good latency timer and cache line size value. */
 	(void) pci_write_config_byte(dev, PCI_LATENCY_TIMER, 64);
 	/* FIXME: pci_set_master() to ensure a good latency timer value */
@@ -425,8 +409,8 @@ static const struct ide_dma_ops cmd648_dma_ops = {
 };
 
 static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
-	{	/* 0 */
-		.name		= "CMD643",
+	{	/* 0: CMD643 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_cmd64x,
 		.enablebits	= {{0x00,0x00,0x00}, {0x51,0x08,0x08}},
 		.port_ops	= &cmd64x_port_ops,
@@ -436,8 +420,9 @@ static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO5,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= 0x00, /* no udma */
-	},{	/* 1 */
-		.name		= "CMD646",
+	},
+	{	/* 1: CMD646 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_cmd64x,
 		.enablebits	= {{0x51,0x04,0x04}, {0x51,0x08,0x08}},
 		.chipset	= ide_cmd646,
@@ -447,8 +432,9 @@ static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO5,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA2,
-	},{	/* 2 */
-		.name		= "CMD648",
+	},
+	{	/* 2: CMD648 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_cmd64x,
 		.enablebits	= {{0x51,0x04,0x04}, {0x51,0x08,0x08}},
 		.port_ops	= &cmd64x_port_ops,
@@ -457,8 +443,9 @@ static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
 		.pio_mask	= ATA_PIO5,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA4,
-	},{	/* 3 */
-		.name		= "CMD649",
+	},
+	{	/* 3: CMD649 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_cmd64x,
 		.enablebits	= {{0x51,0x04,0x04}, {0x51,0x08,0x08}},
 		.port_ops	= &cmd64x_port_ops,
@@ -507,7 +494,7 @@ static int __devinit cmd64x_init_one(struct pci_dev *dev, const struct pci_devic
 		}
 	}
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 static const struct pci_device_id cmd64x_pci_tbl[] = {
@@ -523,6 +510,7 @@ static struct pci_driver driver = {
 	.name		= "CMD64x_IDE",
 	.id_table	= cmd64x_pci_tbl,
 	.probe		= cmd64x_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init cmd64x_ide_init(void)
@@ -530,7 +518,13 @@ static int __init cmd64x_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit cmd64x_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(cmd64x_ide_init);
+module_exit(cmd64x_ide_exit);
 
 MODULE_AUTHOR("Eddie Dost, David Miller, Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for CMD64x IDE");
diff --git a/drivers/ide/pci/cs5520.c b/drivers/ide/pci/cs5520.c
index b03d8ae947e..c0364b287f1 100644
--- a/drivers/ide/pci/cs5520.c
+++ b/drivers/ide/pci/cs5520.c
@@ -41,6 +41,8 @@
 #include <linux/ide.h>
 #include <linux/dma-mapping.h>
 
+#define DRV_NAME "cs5520"
+
 struct pio_clocks
 {
 	int address;
@@ -92,18 +94,11 @@ static const struct ide_port_ops cs5520_port_ops = {
 	.set_dma_mode		= cs5520_set_dma_mode,
 };
 
-#define DECLARE_CS_DEV(name_str)				\
-	{							\
-		.name		= name_str,			\
-		.port_ops	= &cs5520_port_ops,		\
-		.host_flags	= IDE_HFLAG_ISA_PORTS |		\
-				  IDE_HFLAG_CS5520,		\
-		.pio_mask	= ATA_PIO4,			\
-	}
-
-static const struct ide_port_info cyrix_chipsets[] __devinitdata = {
-	/* 0 */ DECLARE_CS_DEV("Cyrix 5510"),
-	/* 1 */ DECLARE_CS_DEV("Cyrix 5520")
+static const struct ide_port_info cyrix_chipset __devinitdata = {
+	.name		= DRV_NAME,
+	.port_ops	= &cs5520_port_ops,
+	.host_flags	= IDE_HFLAG_ISA_PORTS | IDE_HFLAG_CS5520,
+	.pio_mask	= ATA_PIO4,
 };
 
 /*
@@ -114,7 +109,7 @@ static const struct ide_port_info cyrix_chipsets[] __devinitdata = {
  
 static int __devinit cs5520_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	const struct ide_port_info *d = &cyrix_chipsets[id->driver_data];
+	const struct ide_port_info *d = &cyrix_chipset;
 	hw_regs_t hw[4], *hws[] = { NULL, NULL, NULL, NULL };
 
 	ide_setup_pci_noise(dev, d);
@@ -128,7 +123,8 @@ static int __devinit cs5520_init_one(struct pci_dev *dev, const struct pci_devic
 	}
 	pci_set_master(dev);
 	if (pci_set_dma_mask(dev, DMA_32BIT_MASK)) {
-		printk(KERN_WARNING "cs5520: No suitable DMA available.\n");
+		printk(KERN_WARNING "%s: No suitable DMA available.\n",
+			d->name);
 		return -ENODEV;
 	}
 
diff --git a/drivers/ide/pci/cs5530.c b/drivers/ide/pci/cs5530.c
index f5534c1ff34..f235db8c678 100644
--- a/drivers/ide/pci/cs5530.c
+++ b/drivers/ide/pci/cs5530.c
@@ -22,6 +22,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "cs5530"
+
 /*
  * Here are the standard PIO mode 0-4 timings for each "format".
  * Format-0 uses fast data reg timings, with slower command reg timings.
@@ -127,12 +129,11 @@ static void cs5530_set_dma_mode(ide_drive_t *drive, const u8 mode)
 /**
  *	init_chipset_5530	-	set up 5530 bridge
  *	@dev: PCI device
- *	@name: device name
  *
  *	Initialize the cs5530 bridge for reliable IDE DMA operation.
  */
 
-static unsigned int __devinit init_chipset_cs5530 (struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_cs5530(struct pci_dev *dev)
 {
 	struct pci_dev *master_0 = NULL, *cs5530_0 = NULL;
 
@@ -151,11 +152,11 @@ static unsigned int __devinit init_chipset_cs5530 (struct pci_dev *dev, const ch
 		}
 	}
 	if (!master_0) {
-		printk(KERN_ERR "%s: unable to locate PCI MASTER function\n", name);
+		printk(KERN_ERR DRV_NAME ": unable to locate PCI MASTER function\n");
 		goto out;
 	}
 	if (!cs5530_0) {
-		printk(KERN_ERR "%s: unable to locate CS5530 LEGACY function\n", name);
+		printk(KERN_ERR DRV_NAME ": unable to locate CS5530 LEGACY function\n");
 		goto out;
 	}
 
@@ -243,7 +244,7 @@ static const struct ide_port_ops cs5530_port_ops = {
 };
 
 static const struct ide_port_info cs5530_chipset __devinitdata = {
-	.name		= "CS5530",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_cs5530,
 	.init_hwif	= init_hwif_cs5530,
 	.port_ops	= &cs5530_port_ops,
@@ -256,7 +257,7 @@ static const struct ide_port_info cs5530_chipset __devinitdata = {
 
 static int __devinit cs5530_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &cs5530_chipset);
+	return ide_pci_init_one(dev, &cs5530_chipset, NULL);
 }
 
 static const struct pci_device_id cs5530_pci_tbl[] = {
@@ -269,6 +270,7 @@ static struct pci_driver driver = {
 	.name		= "CS5530 IDE",
 	.id_table	= cs5530_pci_tbl,
 	.probe		= cs5530_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init cs5530_ide_init(void)
@@ -276,7 +278,13 @@ static int __init cs5530_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit cs5530_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(cs5530_ide_init);
+module_exit(cs5530_ide_exit);
 
 MODULE_AUTHOR("Mark Lord");
 MODULE_DESCRIPTION("PCI driver module for Cyrix/NS 5530 IDE");
diff --git a/drivers/ide/pci/cs5535.c b/drivers/ide/pci/cs5535.c
index 5404fe4f701..f7b50cdeefa 100644
--- a/drivers/ide/pci/cs5535.c
+++ b/drivers/ide/pci/cs5535.c
@@ -26,6 +26,8 @@
 #include <linux/pci.h>
 #include <linux/ide.h>
 
+#define DRV_NAME "cs5535"
+
 #define MSR_ATAC_BASE		0x51300000
 #define ATAC_GLD_MSR_CAP	(MSR_ATAC_BASE+0)
 #define ATAC_GLD_MSR_CONFIG	(MSR_ATAC_BASE+0x01)
@@ -169,7 +171,7 @@ static const struct ide_port_ops cs5535_port_ops = {
 };
 
 static const struct ide_port_info cs5535_chipset __devinitdata = {
-	.name		= "CS5535",
+	.name		= DRV_NAME,
 	.port_ops	= &cs5535_port_ops,
 	.host_flags	= IDE_HFLAG_SINGLE | IDE_HFLAG_POST_SET_MODE,
 	.pio_mask	= ATA_PIO4,
@@ -180,7 +182,7 @@ static const struct ide_port_info cs5535_chipset __devinitdata = {
 static int __devinit cs5535_init_one(struct pci_dev *dev,
 					const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &cs5535_chipset);
+	return ide_pci_init_one(dev, &cs5535_chipset, NULL);
 }
 
 static const struct pci_device_id cs5535_pci_tbl[] = {
@@ -194,6 +196,7 @@ static struct pci_driver driver = {
 	.name       = "CS5535_IDE",
 	.id_table   = cs5535_pci_tbl,
 	.probe      = cs5535_init_one,
+	.remove     = ide_pci_remove,
 };
 
 static int __init cs5535_ide_init(void)
@@ -201,7 +204,13 @@ static int __init cs5535_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit cs5535_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(cs5535_ide_init);
+module_exit(cs5535_ide_exit);
 
 MODULE_AUTHOR("AMD");
 MODULE_DESCRIPTION("PCI driver module for AMD/NS CS5535 IDE");
diff --git a/drivers/ide/pci/cy82c693.c b/drivers/ide/pci/cy82c693.c
index e14ad5530fa..bfae2f882f4 100644
--- a/drivers/ide/pci/cy82c693.c
+++ b/drivers/ide/pci/cy82c693.c
@@ -48,6 +48,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "cy82c693"
+
 /* the current version */
 #define CY82_VERSION	"CY82C693U driver v0.34 99-13-12 Andreas S. Krebs (akrebs@altavista.net)"
 
@@ -330,7 +332,7 @@ static void cy82c693_set_pio_mode(ide_drive_t *drive, const u8 pio)
 /*
  * this function is called during init and is used to setup the cy82c693 chip
  */
-static unsigned int __devinit init_chipset_cy82c693(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_cy82c693(struct pci_dev *dev)
 {
 	if (PCI_FUNC(dev->devfn) != 1)
 		return 0;
@@ -349,8 +351,8 @@ static unsigned int __devinit init_chipset_cy82c693(struct pci_dev *dev, const c
 	data = inb(CY82_DATA_PORT);
 
 #if CY82C693_DEBUG_INFO
-	printk(KERN_INFO "%s: Peripheral Configuration Register: 0x%X\n",
-		name, data);
+	printk(KERN_INFO DRV_NAME ": Peripheral Configuration Register: 0x%X\n",
+		data);
 #endif /* CY82C693_DEBUG_INFO */
 
 	/*
@@ -371,8 +373,8 @@ static unsigned int __devinit init_chipset_cy82c693(struct pci_dev *dev, const c
 	outb(data, CY82_DATA_PORT);
 
 #if CY82C693_DEBUG_INFO
-	printk(KERN_INFO "%s: New Peripheral Configuration Register: 0x%X\n",
-		name, data);
+	printk(KERN_INFO ": New Peripheral Configuration Register: 0x%X\n",
+		data);
 #endif /* CY82C693_DEBUG_INFO */
 
 #endif /* CY82C693_SETDMA_CLOCK */
@@ -398,7 +400,7 @@ static const struct ide_port_ops cy82c693_port_ops = {
 };
 
 static const struct ide_port_info cy82c693_chipset __devinitdata = {
-	.name		= "CY82C693",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_cy82c693,
 	.init_iops	= init_iops_cy82c693,
 	.port_ops	= &cy82c693_port_ops,
@@ -419,12 +421,22 @@ static int __devinit cy82c693_init_one(struct pci_dev *dev, const struct pci_dev
 	if ((dev->class >> 8) == PCI_CLASS_STORAGE_IDE &&
 	    PCI_FUNC(dev->devfn) == 1) {
 		dev2 = pci_get_slot(dev->bus, dev->devfn + 1);
-		ret = ide_setup_pci_devices(dev, dev2, &cy82c693_chipset);
-		/* We leak pci refs here but thats ok - we can't be unloaded */
+		ret = ide_pci_init_two(dev, dev2, &cy82c693_chipset, NULL);
+		if (ret)
+			pci_dev_put(dev2);
 	}
 	return ret;
 }
 
+static void __devexit cy82c693_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct pci_dev *dev2 = host->dev[1] ? to_pci_dev(host->dev[1]) : NULL;
+
+	ide_pci_remove(dev);
+	pci_dev_put(dev2);
+}
+
 static const struct pci_device_id cy82c693_pci_tbl[] = {
 	{ PCI_VDEVICE(CONTAQ, PCI_DEVICE_ID_CONTAQ_82C693), 0 },
 	{ 0, },
@@ -435,6 +447,7 @@ static struct pci_driver driver = {
 	.name		= "Cypress_IDE",
 	.id_table	= cy82c693_pci_tbl,
 	.probe		= cy82c693_init_one,
+	.remove		= cy82c693_remove,
 };
 
 static int __init cy82c693_ide_init(void)
@@ -442,7 +455,13 @@ static int __init cy82c693_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit cy82c693_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(cy82c693_ide_init);
+module_exit(cy82c693_ide_exit);
 
 MODULE_AUTHOR("Andreas Krebs, Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for the Cypress CY82C693 IDE");
diff --git a/drivers/ide/pci/generic.c b/drivers/ide/pci/generic.c
index 041720e2276..b07d4f4273b 100644
--- a/drivers/ide/pci/generic.c
+++ b/drivers/ide/pci/generic.c
@@ -27,6 +27,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "ide_pci_generic"
+
 static int ide_generic_all;		/* Set to claim all devices */
 
 module_param_named(all_generic_ide, ide_generic_all, bool, 0444);
@@ -34,9 +36,9 @@ MODULE_PARM_DESC(all_generic_ide, "IDE generic will claim all unknown PCI IDE st
 
 #define IDE_HFLAGS_UMC (IDE_HFLAG_NO_DMA | IDE_HFLAG_FORCE_LEGACY_IRQS)
 
-#define DECLARE_GENERIC_PCI_DEV(name_str, extra_flags) \
+#define DECLARE_GENERIC_PCI_DEV(extra_flags) \
 	{ \
-		.name		= name_str, \
+		.name		= DRV_NAME, \
 		.host_flags	= IDE_HFLAG_TRUST_BIOS_FOR_DMA | \
 				  extra_flags, \
 		.swdma_mask	= ATA_SWDMA2, \
@@ -45,10 +47,11 @@ MODULE_PARM_DESC(all_generic_ide, "IDE generic will claim all unknown PCI IDE st
 	}
 
 static const struct ide_port_info generic_chipsets[] __devinitdata = {
-	/*  0 */ DECLARE_GENERIC_PCI_DEV("Unknown",	0),
+	/*  0: Unknown */
+	DECLARE_GENERIC_PCI_DEV(0),
 
-	{	/* 1 */
-		.name		= "NS87410",
+	{	/* 1: NS87410 */
+		.name		= DRV_NAME,
 		.enablebits	= { {0x43, 0x08, 0x08}, {0x47, 0x08, 0x08} },
 		.host_flags	= IDE_HFLAG_TRUST_BIOS_FOR_DMA,
 		.swdma_mask	= ATA_SWDMA2,
@@ -56,17 +59,15 @@ static const struct ide_port_info generic_chipsets[] __devinitdata = {
 		.udma_mask	= ATA_UDMA6,
 	},
 
-	/*  2 */ DECLARE_GENERIC_PCI_DEV("SAMURAI",	0),
-	/*  3 */ DECLARE_GENERIC_PCI_DEV("HT6565",	0),
-	/*  4 */ DECLARE_GENERIC_PCI_DEV("UM8673F",	IDE_HFLAGS_UMC),
-	/*  5 */ DECLARE_GENERIC_PCI_DEV("UM8886A",	IDE_HFLAGS_UMC),
-	/*  6 */ DECLARE_GENERIC_PCI_DEV("UM8886BF",	IDE_HFLAGS_UMC),
-	/*  7 */ DECLARE_GENERIC_PCI_DEV("HINT_IDE",	0),
-	/*  8 */ DECLARE_GENERIC_PCI_DEV("VIA_IDE",	IDE_HFLAG_NO_AUTODMA),
-	/*  9 */ DECLARE_GENERIC_PCI_DEV("OPTI621V",	IDE_HFLAG_NO_AUTODMA),
-
-	{	/* 10 */
-		.name		= "VIA8237SATA",
+	/*  2: SAMURAI / HT6565 / HINT_IDE */
+	DECLARE_GENERIC_PCI_DEV(0),
+	/*  3: UM8673F / UM8886A / UM8886BF */
+	DECLARE_GENERIC_PCI_DEV(IDE_HFLAGS_UMC),
+	/*  4: VIA_IDE / OPTI621V / Piccolo010{2,3,5} */
+	DECLARE_GENERIC_PCI_DEV(IDE_HFLAG_NO_AUTODMA),
+
+	{	/* 5: VIA8237SATA */
+		.name		= DRV_NAME,
 		.host_flags	= IDE_HFLAG_TRUST_BIOS_FOR_DMA |
 				  IDE_HFLAG_OFF_BOARD,
 		.swdma_mask	= ATA_SWDMA2,
@@ -74,12 +75,8 @@ static const struct ide_port_info generic_chipsets[] __devinitdata = {
 		.udma_mask	= ATA_UDMA6,
 	},
 
-	/* 11 */ DECLARE_GENERIC_PCI_DEV("Piccolo0102",	IDE_HFLAG_NO_AUTODMA),
-	/* 12 */ DECLARE_GENERIC_PCI_DEV("Piccolo0103",	IDE_HFLAG_NO_AUTODMA),
-	/* 13 */ DECLARE_GENERIC_PCI_DEV("Piccolo0105",	IDE_HFLAG_NO_AUTODMA),
-
-	{	/* 14 */
-		.name		= "Revolution",
+	{	/* 6: Revolution */
+		.name		= DRV_NAME,
 		.host_flags	= IDE_HFLAG_CLEAR_SIMPLEX |
 				  IDE_HFLAG_TRUST_BIOS_FOR_DMA |
 				  IDE_HFLAG_OFF_BOARD,
@@ -134,12 +131,12 @@ static int __devinit generic_init_one(struct pci_dev *dev, const struct pci_devi
 		u16 command;
 		pci_read_config_word(dev, PCI_COMMAND, &command);
 		if (!(command & PCI_COMMAND_IO)) {
-			printk(KERN_INFO "Skipping disabled %s IDE "
-					"controller.\n", d->name);
+			printk(KERN_INFO "%s %s: skipping disabled "
+				"controller\n", d->name, pci_name(dev));
 			goto out;
 		}
 	}
-	ret = ide_setup_pci_device(dev, d);
+	ret = ide_pci_init_one(dev, d, NULL);
 out:
 	return ret;
 }
@@ -147,20 +144,20 @@ out:
 static const struct pci_device_id generic_pci_tbl[] = {
 	{ PCI_VDEVICE(NS,	PCI_DEVICE_ID_NS_87410),		 1 },
 	{ PCI_VDEVICE(PCTECH,	PCI_DEVICE_ID_PCTECH_SAMURAI_IDE),	 2 },
-	{ PCI_VDEVICE(HOLTEK,	PCI_DEVICE_ID_HOLTEK_6565),		 3 },
-	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8673F),		 4 },
-	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8886A),		 5 },
-	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8886BF),		 6 },
-	{ PCI_VDEVICE(HINT,	PCI_DEVICE_ID_HINT_VXPROII_IDE),	 7 },
-	{ PCI_VDEVICE(VIA,	PCI_DEVICE_ID_VIA_82C561),		 8 },
-	{ PCI_VDEVICE(OPTI,	PCI_DEVICE_ID_OPTI_82C558),		 9 },
+	{ PCI_VDEVICE(HOLTEK,	PCI_DEVICE_ID_HOLTEK_6565),		 2 },
+	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8673F),		 3 },
+	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8886A),		 3 },
+	{ PCI_VDEVICE(UMC,	PCI_DEVICE_ID_UMC_UM8886BF),		 3 },
+	{ PCI_VDEVICE(HINT,	PCI_DEVICE_ID_HINT_VXPROII_IDE),	 2 },
+	{ PCI_VDEVICE(VIA,	PCI_DEVICE_ID_VIA_82C561),		 4 },
+	{ PCI_VDEVICE(OPTI,	PCI_DEVICE_ID_OPTI_82C558),		 4 },
 #ifdef CONFIG_BLK_DEV_IDE_SATA
-	{ PCI_VDEVICE(VIA,	PCI_DEVICE_ID_VIA_8237_SATA),		10 },
+	{ PCI_VDEVICE(VIA,	PCI_DEVICE_ID_VIA_8237_SATA),		 5 },
 #endif
-	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO),		11 },
-	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO_1),	12 },
-	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO_2),	13 },
-	{ PCI_VDEVICE(NETCELL,	PCI_DEVICE_ID_REVOLUTION),		14 },
+	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO),		 4 },
+	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO_1),	 4 },
+	{ PCI_VDEVICE(TOSHIBA,	PCI_DEVICE_ID_TOSHIBA_PICCOLO_2),	 4 },
+	{ PCI_VDEVICE(NETCELL,	PCI_DEVICE_ID_REVOLUTION),		 6 },
 	/*
 	 * Must come last.  If you add entries adjust
 	 * this table and generic_chipsets[] appropriately.
@@ -174,6 +171,7 @@ static struct pci_driver driver = {
 	.name		= "PCI_IDE",
 	.id_table	= generic_pci_tbl,
 	.probe		= generic_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init generic_ide_init(void)
@@ -181,7 +179,13 @@ static int __init generic_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit generic_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(generic_ide_init);
+module_exit(generic_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for generic PCI IDE");
diff --git a/drivers/ide/pci/hpt34x.c b/drivers/ide/pci/hpt34x.c
index 9e1d1c4741d..6009b0b9655 100644
--- a/drivers/ide/pci/hpt34x.c
+++ b/drivers/ide/pci/hpt34x.c
@@ -33,6 +33,8 @@
 #include <linux/init.h>
 #include <linux/ide.h>
 
+#define DRV_NAME "hpt34x"
+
 #define HPT343_DEBUG_DRIVE_INFO		0
 
 static void hpt34x_set_mode(ide_drive_t *drive, const u8 speed)
@@ -77,7 +79,7 @@ static void hpt34x_set_pio_mode(ide_drive_t *drive, const u8 pio)
  */
 #define	HPT34X_PCI_INIT_REG		0x80
 
-static unsigned int __devinit init_chipset_hpt34x(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_hpt34x(struct pci_dev *dev)
 {
 	int i = 0;
 	unsigned long hpt34xIoBase = pci_resource_start(dev, 4);
@@ -126,15 +128,15 @@ static const struct ide_port_ops hpt34x_port_ops = {
 	 IDE_HFLAG_NO_AUTODMA)
 
 static const struct ide_port_info hpt34x_chipsets[] __devinitdata = {
-	{ /* 0 */
-		.name		= "HPT343",
+	{ /* 0: HPT343 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_hpt34x,
 		.port_ops	= &hpt34x_port_ops,
 		.host_flags	= IDE_HFLAGS_HPT34X | IDE_HFLAG_NON_BOOTABLE,
 		.pio_mask	= ATA_PIO5,
 	},
-	{ /* 1 */
-		.name		= "HPT345",
+	{ /* 1: HPT345 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_hpt34x,
 		.port_ops	= &hpt34x_port_ops,
 		.host_flags	= IDE_HFLAGS_HPT34X | IDE_HFLAG_OFF_BOARD,
@@ -156,7 +158,7 @@ static int __devinit hpt34x_init_one(struct pci_dev *dev, const struct pci_devic
 
 	d = &hpt34x_chipsets[(pcicmd & PCI_COMMAND_MEMORY) ? 1 : 0];
 
-	return ide_setup_pci_device(dev, d);
+	return ide_pci_init_one(dev, d, NULL);
 }
 
 static const struct pci_device_id hpt34x_pci_tbl[] = {
@@ -169,6 +171,7 @@ static struct pci_driver driver = {
 	.name		= "HPT34x_IDE",
 	.id_table	= hpt34x_pci_tbl,
 	.probe		= hpt34x_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init hpt34x_ide_init(void)
@@ -176,7 +179,13 @@ static int __init hpt34x_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit hpt34x_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(hpt34x_ide_init);
+module_exit(hpt34x_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for Highpoint 34x IDE");
diff --git a/drivers/ide/pci/hpt366.c b/drivers/ide/pci/hpt366.c
index 1f1135ce7cd..5271b246b88 100644
--- a/drivers/ide/pci/hpt366.c
+++ b/drivers/ide/pci/hpt366.c
@@ -131,6 +131,8 @@
 #include <asm/uaccess.h>
 #include <asm/io.h>
 
+#define DRV_NAME "hpt366"
+
 /* various tuning parameters */
 #define HPT_RESET_STATE_ENGINE
 #undef	HPT_DELAY_INTERRUPT
@@ -620,7 +622,8 @@ static u8 hpt3xx_udma_filter(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
-	struct hpt_info *info	= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 	u8 mask 		= hwif->ultra_mask;
 
 	switch (info->chip_type) {
@@ -660,7 +663,8 @@ static u8 hpt3xx_mdma_filter(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
-	struct hpt_info *info	= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 
 	switch (info->chip_type) {
 	case HPT372 :
@@ -694,8 +698,10 @@ static u32 get_speed_setting(u8 speed, struct hpt_info *info)
 
 static void hpt3xx_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	struct pci_dev  *dev	= to_pci_dev(drive->hwif->dev);
-	struct hpt_info	*info	= pci_get_drvdata(dev);
+	ide_hwif_t *hwif	= drive->hwif;
+	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 	struct hpt_timings *t	= info->timings;
 	u8  itr_addr		= 0x40 + (drive->dn * 4);
 	u32 old_itr		= 0;
@@ -738,7 +744,8 @@ static void hpt3xx_maskproc(ide_drive_t *drive, int mask)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
 	struct pci_dev	*dev	= to_pci_dev(hwif->dev);
-	struct hpt_info *info	= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 
 	if (drive->quirk_list) {
 		if (info->chip_type >= HPT370) {
@@ -963,24 +970,16 @@ static int __devinit hpt37x_calibrate_dpll(struct pci_dev *dev, u16 f_low, u16 f
 	return 1;
 }
 
-static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev)
 {
-	struct hpt_info *info	= kmalloc(sizeof(struct hpt_info), GFP_KERNEL);
 	unsigned long io_base	= pci_resource_start(dev, 4);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (&dev->dev == host->dev[1]);
+	const char *name	= DRV_NAME;
 	u8 pci_clk,  dpll_clk	= 0;	/* PCI and DPLL clock in MHz */
 	u8 chip_type;
 	enum ata_clock	clock;
 
-	if (info == NULL) {
-		printk(KERN_ERR "%s: out of memory!\n", name);
-		return -ENOMEM;
-	}
-
-	/*
-	 * Copy everything from a static "template" structure
-	 * to just allocated per-chip hpt_info structure.
-	 */
-	memcpy(info, pci_get_drvdata(dev), sizeof(struct hpt_info));
 	chip_type = info->chip_type;
 
 	pci_write_config_byte(dev, PCI_CACHE_LINE_SIZE, (L1_CACHE_BYTES / 4));
@@ -1048,8 +1047,8 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 		if ((temp & 0xFFFFF000) != 0xABCDE000) {
 			int i;
 
-			printk(KERN_WARNING "%s: no clock data saved by BIOS\n",
-			       name);
+			printk(KERN_WARNING "%s %s: no clock data saved by "
+				"BIOS\n", name, pci_name(dev));
 
 			/* Calculate the average value of f_CNT. */
 			for (temp = i = 0; i < 128; i++) {
@@ -1074,8 +1073,9 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 		else
 			pci_clk = 66;
 
-		printk(KERN_INFO "%s: DPLL base: %d MHz, f_CNT: %d, "
-		       "assuming %d MHz PCI\n", name, dpll_clk, f_cnt, pci_clk);
+		printk(KERN_INFO "%s %s: DPLL base: %d MHz, f_CNT: %d, "
+			"assuming %d MHz PCI\n", name, pci_name(dev),
+			dpll_clk, f_cnt, pci_clk);
 	} else {
 		u32 itr1 = 0;
 
@@ -1141,8 +1141,8 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 		}
 
 		if (info->timings->clock_table[clock] == NULL) {
-			printk(KERN_ERR "%s: unknown bus timing!\n", name);
-			kfree(info);
+			printk(KERN_ERR "%s %s: unknown bus timing!\n",
+				name, pci_name(dev));
 			return -EIO;
 		}
 
@@ -1168,17 +1168,19 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 				f_low += adjust >> 1;
 		}
 		if (adjust == 8) {
-			printk(KERN_ERR "%s: DPLL did not stabilize!\n", name);
-			kfree(info);
+			printk(KERN_ERR "%s %s: DPLL did not stabilize!\n",
+				name, pci_name(dev));
 			return -EIO;
 		}
 
-		printk("%s: using %d MHz DPLL clock\n", name, dpll_clk);
+		printk(KERN_INFO "%s %s: using %d MHz DPLL clock\n",
+			name, pci_name(dev), dpll_clk);
 	} else {
 		/* Mark the fact that we're not using the DPLL. */
 		dpll_clk = 0;
 
-		printk("%s: using %d MHz PCI clock\n", name, pci_clk);
+		printk(KERN_INFO "%s %s: using %d MHz PCI clock\n",
+			name, pci_name(dev), pci_clk);
 	}
 
 	/* Store the clock frequencies. */
@@ -1186,9 +1188,6 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 	info->pci_clk	= pci_clk;
 	info->clock	= clock;
 
-	/* Point to this chip's own instance of the hpt_info structure. */
-	pci_set_drvdata(dev, info);
-
 	if (chip_type >= HPT370) {
 		u8  mcr1, mcr4;
 
@@ -1218,7 +1217,8 @@ static unsigned int __devinit init_chipset_hpt366(struct pci_dev *dev, const cha
 static u8 __devinit hpt3xx_cable_detect(ide_hwif_t *hwif)
 {
 	struct pci_dev	*dev	= to_pci_dev(hwif->dev);
-	struct hpt_info *info	= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 	u8 chip_type		= info->chip_type;
 	u8 scr1 = 0, ata66	= hwif->channel ? 0x01 : 0x02;
 
@@ -1262,7 +1262,8 @@ static u8 __devinit hpt3xx_cable_detect(ide_hwif_t *hwif)
 static void __devinit init_hwif_hpt366(ide_hwif_t *hwif)
 {
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
-	struct hpt_info *info	= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	struct hpt_info *info	= host->host_priv + (hwif->dev == host->dev[1]);
 	int serialize		= HPT_SERIALIZE_IO;
 	u8  chip_type		= info->chip_type;
 	u8  new_mcr, old_mcr	= 0;
@@ -1364,7 +1365,8 @@ static void __devinit hpt374_init(struct pci_dev *dev, struct pci_dev *dev2)
 	if (dev2->irq != dev->irq) {
 		/* FIXME: we need a core pci_set_interrupt() */
 		dev2->irq = dev->irq;
-		printk(KERN_INFO "HPT374: PCI config space interrupt fixed\n");
+		printk(KERN_INFO DRV_NAME " %s: PCI config space interrupt "
+			"fixed\n", pci_name(dev2));
 	}
 }
 
@@ -1399,8 +1401,8 @@ static int __devinit hpt36x_init(struct pci_dev *dev, struct pci_dev *dev2)
 	pci_read_config_byte(dev2, PCI_INTERRUPT_PIN, &pin2);
 
 	if (pin1 != pin2 && dev->irq == dev2->irq) {
-		printk(KERN_INFO "HPT36x: onboard version of chipset, "
-				 "pin1=%d pin2=%d\n", pin1, pin2);
+		printk(KERN_INFO DRV_NAME " %s: onboard version of chipset, "
+			"pin1=%d pin2=%d\n", pci_name(dev), pin1, pin2);
 		return 1;
 	}
 
@@ -1455,8 +1457,8 @@ static const struct ide_dma_ops hpt36x_dma_ops = {
 };
 
 static const struct ide_port_info hpt366_chipsets[] __devinitdata = {
-	{	/* 0 */
-		.name		= "HPT36x",
+	{	/* 0: HPT36x */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_hpt366,
 		.init_hwif	= init_hwif_hpt366,
 		.init_dma	= init_dma_hpt366,
@@ -1472,53 +1474,9 @@ static const struct ide_port_info hpt366_chipsets[] __devinitdata = {
 		.host_flags	= IDE_HFLAGS_HPT3XX | IDE_HFLAG_SINGLE,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
-	},{	/* 1 */
-		.name		= "HPT372A",
-		.init_chipset	= init_chipset_hpt366,
-		.init_hwif	= init_hwif_hpt366,
-		.init_dma	= init_dma_hpt366,
-		.enablebits	= {{0x50,0x04,0x04}, {0x54,0x04,0x04}},
-		.port_ops	= &hpt3xx_port_ops,
-		.dma_ops	= &hpt37x_dma_ops,
-		.host_flags	= IDE_HFLAGS_HPT3XX,
-		.pio_mask	= ATA_PIO4,
-		.mwdma_mask	= ATA_MWDMA2,
-	},{	/* 2 */
-		.name		= "HPT302",
-		.init_chipset	= init_chipset_hpt366,
-		.init_hwif	= init_hwif_hpt366,
-		.init_dma	= init_dma_hpt366,
-		.enablebits	= {{0x50,0x04,0x04}, {0x54,0x04,0x04}},
-		.port_ops	= &hpt3xx_port_ops,
-		.dma_ops	= &hpt37x_dma_ops,
-		.host_flags	= IDE_HFLAGS_HPT3XX,
-		.pio_mask	= ATA_PIO4,
-		.mwdma_mask	= ATA_MWDMA2,
-	},{	/* 3 */
-		.name		= "HPT371",
-		.init_chipset	= init_chipset_hpt366,
-		.init_hwif	= init_hwif_hpt366,
-		.init_dma	= init_dma_hpt366,
-		.enablebits	= {{0x50,0x04,0x04}, {0x54,0x04,0x04}},
-		.port_ops	= &hpt3xx_port_ops,
-		.dma_ops	= &hpt37x_dma_ops,
-		.host_flags	= IDE_HFLAGS_HPT3XX,
-		.pio_mask	= ATA_PIO4,
-		.mwdma_mask	= ATA_MWDMA2,
-	},{	/* 4 */
-		.name		= "HPT374",
-		.init_chipset	= init_chipset_hpt366,
-		.init_hwif	= init_hwif_hpt366,
-		.init_dma	= init_dma_hpt366,
-		.enablebits	= {{0x50,0x04,0x04}, {0x54,0x04,0x04}},
-		.udma_mask	= ATA_UDMA5,
-		.port_ops	= &hpt3xx_port_ops,
-		.dma_ops	= &hpt37x_dma_ops,
-		.host_flags	= IDE_HFLAGS_HPT3XX,
-		.pio_mask	= ATA_PIO4,
-		.mwdma_mask	= ATA_MWDMA2,
-	},{	/* 5 */
-		.name		= "HPT372N",
+	},
+	{	/* 1: HPT3xx */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_hpt366,
 		.init_hwif	= init_hwif_hpt366,
 		.init_dma	= init_dma_hpt366,
@@ -1542,10 +1500,12 @@ static const struct ide_port_info hpt366_chipsets[] __devinitdata = {
 static int __devinit hpt366_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	const struct hpt_info *info = NULL;
+	struct hpt_info *dyn_info;
 	struct pci_dev *dev2 = NULL;
 	struct ide_port_info d;
 	u8 idx = id->driver_data;
 	u8 rev = dev->revision;
+	int ret;
 
 	if ((idx == 0 || idx == 4) && (PCI_FUNC(dev->devfn) & 1))
 		return -ENODEV;
@@ -1582,24 +1542,35 @@ static int __devinit hpt366_init_one(struct pci_dev *dev, const struct pci_devic
 		break;
 	}
 
-	d = hpt366_chipsets[idx];
+	printk(KERN_INFO DRV_NAME ": %s chipset detected\n", info->chip_name);
+
+	d = hpt366_chipsets[min_t(u8, idx, 1)];
 
-	d.name = info->chip_name;
 	d.udma_mask = info->udma_mask;
 
 	/* fixup ->dma_ops for HPT370/HPT370A */
 	if (info == &hpt370 || info == &hpt370a)
 		d.dma_ops = &hpt370_dma_ops;
 
-	pci_set_drvdata(dev, (void *)info);
-
 	if (info == &hpt36x || info == &hpt374)
 		dev2 = pci_get_slot(dev->bus, dev->devfn + 1);
 
-	if (dev2) {
-		int ret;
+	dyn_info = kzalloc(sizeof(*dyn_info) * (dev2 ? 2 : 1), GFP_KERNEL);
+	if (dyn_info == NULL) {
+		printk(KERN_ERR "%s %s: out of memory!\n",
+			d.name, pci_name(dev));
+		pci_dev_put(dev2);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Copy everything from a static "template" structure
+	 * to just allocated per-chip hpt_info structure.
+	 */
+	memcpy(dyn_info, info, sizeof(*dyn_info));
 
-		pci_set_drvdata(dev2, (void *)info);
+	if (dev2) {
+		memcpy(dyn_info + 1, info, sizeof(*dyn_info));
 
 		if (info == &hpt374)
 			hpt374_init(dev, dev2);
@@ -1608,13 +1579,30 @@ static int __devinit hpt366_init_one(struct pci_dev *dev, const struct pci_devic
 				d.host_flags &= ~IDE_HFLAG_NON_BOOTABLE;
 		}
 
-		ret = ide_setup_pci_devices(dev, dev2, &d);
-		if (ret < 0)
+		ret = ide_pci_init_two(dev, dev2, &d, dyn_info);
+		if (ret < 0) {
 			pci_dev_put(dev2);
+			kfree(dyn_info);
+		}
 		return ret;
 	}
 
-	return ide_setup_pci_device(dev, &d);
+	ret = ide_pci_init_one(dev, &d, dyn_info);
+	if (ret < 0)
+		kfree(dyn_info);
+
+	return ret;
+}
+
+static void __devexit hpt366_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct ide_info *info = host->host_priv;
+	struct pci_dev *dev2 = host->dev[1] ? to_pci_dev(host->dev[1]) : NULL;
+
+	ide_pci_remove(dev);
+	pci_dev_put(dev2);
+	kfree(info);
 }
 
 static const struct pci_device_id hpt366_pci_tbl[] __devinitconst = {
@@ -1632,6 +1620,7 @@ static struct pci_driver driver = {
 	.name		= "HPT366_IDE",
 	.id_table	= hpt366_pci_tbl,
 	.probe		= hpt366_init_one,
+	.remove		= hpt366_remove,
 };
 
 static int __init hpt366_ide_init(void)
@@ -1639,7 +1628,13 @@ static int __init hpt366_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit hpt366_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(hpt366_ide_init);
+module_exit(hpt366_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for Highpoint HPT366 IDE");
diff --git a/drivers/ide/pci/it8213.c b/drivers/ide/pci/it8213.c
index 2b71bdf74e7..6eba8f18826 100644
--- a/drivers/ide/pci/it8213.c
+++ b/drivers/ide/pci/it8213.c
@@ -14,6 +14,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "it8213"
+
 /**
  *	it8213_set_pio_mode	-	set host controller for PIO mode
  *	@drive: drive
@@ -155,23 +157,17 @@ static const struct ide_port_ops it8213_port_ops = {
 	.cable_detect		= it8213_cable_detect,
 };
 
-#define DECLARE_ITE_DEV(name_str)			\
-	{						\
-		.name		= name_str,		\
-		.enablebits	= { {0x41, 0x80, 0x80} }, \
-		.port_ops	= &it8213_port_ops,	\
-		.host_flags	= IDE_HFLAG_SINGLE,	\
-		.pio_mask	= ATA_PIO4,		\
-		.swdma_mask	= ATA_SWDMA2_ONLY,	\
-		.mwdma_mask	= ATA_MWDMA12_ONLY,	\
-		.udma_mask	= ATA_UDMA6,		\
-	}
-
-static const struct ide_port_info it8213_chipsets[] __devinitdata = {
-	/* 0 */ DECLARE_ITE_DEV("IT8213"),
+static const struct ide_port_info it8213_chipset __devinitdata = {
+	.name		= DRV_NAME,
+	.enablebits	= { {0x41, 0x80, 0x80} },
+	.port_ops	= &it8213_port_ops,
+	.host_flags	= IDE_HFLAG_SINGLE,
+	.pio_mask	= ATA_PIO4,
+	.swdma_mask	= ATA_SWDMA2_ONLY,
+	.mwdma_mask	= ATA_MWDMA12_ONLY,
+	.udma_mask	= ATA_UDMA6,
 };
 
-
 /**
  *	it8213_init_one	-	pci layer discovery entry
  *	@dev: PCI device
@@ -184,7 +180,7 @@ static const struct ide_port_info it8213_chipsets[] __devinitdata = {
 
 static int __devinit it8213_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &it8213_chipsets[id->driver_data]);
+	return ide_pci_init_one(dev, &it8213_chipset, NULL);
 }
 
 static const struct pci_device_id it8213_pci_tbl[] = {
@@ -198,6 +194,7 @@ static struct pci_driver driver = {
 	.name		= "ITE8213_IDE",
 	.id_table	= it8213_pci_tbl,
 	.probe		= it8213_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init it8213_ide_init(void)
@@ -205,7 +202,13 @@ static int __init it8213_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit it8213_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(it8213_ide_init);
+module_exit(it8213_ide_exit);
 
 MODULE_AUTHOR("Jack Lee, Alan Cox");
 MODULE_DESCRIPTION("PCI driver module for the ITE 8213");
diff --git a/drivers/ide/pci/it821x.c b/drivers/ide/pci/it821x.c
index cbf64720299..e16a1d113a2 100644
--- a/drivers/ide/pci/it821x.c
+++ b/drivers/ide/pci/it821x.c
@@ -67,6 +67,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "it821x"
+
 struct it821x_dev
 {
 	unsigned int smart:1,		/* Are we in smart raid mode */
@@ -534,8 +536,9 @@ static struct ide_dma_ops it821x_pass_through_dma_ops = {
 static void __devinit init_hwif_it821x(ide_hwif_t *hwif)
 {
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
-	struct it821x_dev **itdevs = (struct it821x_dev **)pci_get_drvdata(dev);
-	struct it821x_dev *idev = itdevs[hwif->channel];
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct it821x_dev *itdevs = host->host_priv;
+	struct it821x_dev *idev = itdevs + hwif->channel;
 	u8 conf;
 
 	ide_set_hwifdata(hwif, idev);
@@ -568,7 +571,8 @@ static void __devinit init_hwif_it821x(ide_hwif_t *hwif)
 		idev->timing10 = 1;
 		hwif->host_flags |= IDE_HFLAG_NO_ATAPI_DMA;
 		if (idev->smart == 0)
-			printk(KERN_WARNING "it821x: Revision 0x10, workarounds activated.\n");
+			printk(KERN_WARNING DRV_NAME " %s: revision 0x10, "
+				"workarounds activated\n", pci_name(dev));
 	}
 
 	if (idev->smart == 0) {
@@ -601,18 +605,20 @@ static void __devinit it8212_disable_raid(struct pci_dev *dev)
 	pci_write_config_byte(dev, PCI_LATENCY_TIMER, 0x20);
 }
 
-static unsigned int __devinit init_chipset_it821x(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_it821x(struct pci_dev *dev)
 {
 	u8 conf;
 	static char *mode[2] = { "pass through", "smart" };
 
 	/* Force the card into bypass mode if so requested */
 	if (it8212_noraid) {
-		printk(KERN_INFO "it8212: forcing bypass mode.\n");
+		printk(KERN_INFO DRV_NAME " %s: forcing bypass mode\n",
+			pci_name(dev));
 		it8212_disable_raid(dev);
 	}
 	pci_read_config_byte(dev, 0x50, &conf);
-	printk(KERN_INFO "it821x: controller in %s mode.\n", mode[conf & 1]);
+	printk(KERN_INFO DRV_NAME " %s: controller in %s mode\n",
+		pci_name(dev), mode[conf & 1]);
 	return 0;
 }
 
@@ -624,17 +630,12 @@ static const struct ide_port_ops it821x_port_ops = {
 	.cable_detect		= it821x_cable_detect,
 };
 
-#define DECLARE_ITE_DEV(name_str)			\
-	{						\
-		.name		= name_str,		\
-		.init_chipset	= init_chipset_it821x,	\
-		.init_hwif	= init_hwif_it821x,	\
-		.port_ops	= &it821x_port_ops,	\
-		.pio_mask	= ATA_PIO4,		\
-	}
-
-static const struct ide_port_info it821x_chipsets[] __devinitdata = {
-	/* 0 */ DECLARE_ITE_DEV("IT8212"),
+static const struct ide_port_info it821x_chipset __devinitdata = {
+	.name		= DRV_NAME,
+	.init_chipset	= init_chipset_it821x,
+	.init_hwif	= init_hwif_it821x,
+	.port_ops	= &it821x_port_ops,
+	.pio_mask	= ATA_PIO4,
 };
 
 /**
@@ -648,23 +649,29 @@ static const struct ide_port_info it821x_chipsets[] __devinitdata = {
 
 static int __devinit it821x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	struct it821x_dev *itdevs[2] = { NULL, NULL} , *itdev;
-	unsigned int i;
-
-	for (i = 0; i < 2; i++) {
-		itdev = kzalloc(sizeof(*itdev), GFP_KERNEL);
-		if (itdev == NULL) {
-			kfree(itdevs[0]);
-			printk(KERN_ERR "it821x: out of memory\n");
-			return -ENOMEM;
-		}
+	struct it821x_dev *itdevs;
+	int rc;
 
-		itdevs[i] = itdev;
+	itdevs = kzalloc(2 * sizeof(*itdevs), GFP_KERNEL);
+	if (itdevs == NULL) {
+		printk(KERN_ERR DRV_NAME " %s: out of memory\n", pci_name(dev));
+		return -ENOMEM;
 	}
 
-	pci_set_drvdata(dev, itdevs);
+	rc = ide_pci_init_one(dev, &it821x_chipset, itdevs);
+	if (rc)
+		kfree(itdevs);
 
-	return ide_setup_pci_device(dev, &it821x_chipsets[id->driver_data]);
+	return rc;
+}
+
+static void __devexit it821x_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct it821x_dev *itdevs = host->host_priv;
+
+	ide_pci_remove(dev);
+	kfree(itdevs);
 }
 
 static const struct pci_device_id it821x_pci_tbl[] = {
@@ -679,6 +686,7 @@ static struct pci_driver driver = {
 	.name		= "ITE821x IDE",
 	.id_table	= it821x_pci_tbl,
 	.probe		= it821x_init_one,
+	.remove		= it821x_remove,
 };
 
 static int __init it821x_ide_init(void)
@@ -686,7 +694,13 @@ static int __init it821x_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit it821x_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(it821x_ide_init);
+module_exit(it821x_ide_exit);
 
 module_param_named(noraid, it8212_noraid, int, S_IRUGO);
 MODULE_PARM_DESC(noraid, "Force card into bypass mode");
diff --git a/drivers/ide/pci/jmicron.c b/drivers/ide/pci/jmicron.c
index 96ef7394f28..545b6e172d9 100644
--- a/drivers/ide/pci/jmicron.c
+++ b/drivers/ide/pci/jmicron.c
@@ -12,6 +12,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "jmicron"
+
 typedef enum {
 	PORT_PATA0 = 0,
 	PORT_PATA1 = 1,
@@ -102,7 +104,7 @@ static const struct ide_port_ops jmicron_port_ops = {
 };
 
 static const struct ide_port_info jmicron_chipset __devinitdata = {
-	.name		= "JMB",
+	.name		= DRV_NAME,
 	.enablebits	= { { 0x40, 0x01, 0x01 }, { 0x40, 0x10, 0x10 } },
 	.port_ops	= &jmicron_port_ops,
 	.pio_mask	= ATA_PIO5,
@@ -121,7 +123,7 @@ static const struct ide_port_info jmicron_chipset __devinitdata = {
 
 static int __devinit jmicron_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &jmicron_chipset);
+	return ide_pci_init_one(dev, &jmicron_chipset, NULL);
 }
 
 /* All JMB PATA controllers have and will continue to have the same
@@ -152,6 +154,7 @@ static struct pci_driver driver = {
 	.name		= "JMicron IDE",
 	.id_table	= jmicron_pci_tbl,
 	.probe		= jmicron_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init jmicron_ide_init(void)
@@ -159,7 +162,13 @@ static int __init jmicron_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit jmicron_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(jmicron_ide_init);
+module_exit(jmicron_ide_exit);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("PCI driver module for the JMicron in legacy modes");
diff --git a/drivers/ide/pci/ns87415.c b/drivers/ide/pci/ns87415.c
index 5cd2b32ff0e..ffefcd15196 100644
--- a/drivers/ide/pci/ns87415.c
+++ b/drivers/ide/pci/ns87415.c
@@ -19,6 +19,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "ns87415"
+
 #ifdef CONFIG_SUPERIO
 /* SUPERIO 87560 is a PoS chip that NatSem denies exists.
  * Unfortunately, it's built-in on all Astro-based PA-RISC workstations
@@ -305,7 +307,7 @@ static const struct ide_dma_ops ns87415_dma_ops = {
 };
 
 static const struct ide_port_info ns87415_chipset __devinitdata = {
-	.name		= "NS87415",
+	.name		= DRV_NAME,
 	.init_hwif	= init_hwif_ns87415,
 	.port_ops	= &ns87415_port_ops,
 	.dma_ops	= &ns87415_dma_ops,
@@ -324,7 +326,7 @@ static int __devinit ns87415_init_one(struct pci_dev *dev, const struct pci_devi
 		d.tp_ops = &superio_tp_ops;
 	}
 #endif
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 static const struct pci_device_id ns87415_pci_tbl[] = {
@@ -337,6 +339,7 @@ static struct pci_driver driver = {
 	.name		= "NS87415_IDE",
 	.id_table	= ns87415_pci_tbl,
 	.probe		= ns87415_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init ns87415_ide_init(void)
@@ -344,7 +347,13 @@ static int __init ns87415_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit ns87415_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(ns87415_ide_init);
+module_exit(ns87415_ide_exit);
 
 MODULE_AUTHOR("Mark Lord, Eddie Dost, Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for NS87415 IDE");
diff --git a/drivers/ide/pci/opti621.c b/drivers/ide/pci/opti621.c
index 725c80508d9..e28e672ddaf 100644
--- a/drivers/ide/pci/opti621.c
+++ b/drivers/ide/pci/opti621.c
@@ -90,6 +90,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "opti621"
+
 #define READ_REG 0	/* index of Read cycle timing register */
 #define WRITE_REG 1	/* index of Write cycle timing register */
 #define CNTRL_REG 3	/* index of Control register */
@@ -200,7 +202,7 @@ static const struct ide_port_ops opti621_port_ops = {
 };
 
 static const struct ide_port_info opti621_chipset __devinitdata = {
-	.name		= "OPTI621/X",
+	.name		= DRV_NAME,
 	.enablebits	= { {0x45, 0x80, 0x00}, {0x40, 0x08, 0x00} },
 	.port_ops	= &opti621_port_ops,
 	.host_flags	= IDE_HFLAG_NO_DMA,
@@ -209,7 +211,7 @@ static const struct ide_port_info opti621_chipset __devinitdata = {
 
 static int __devinit opti621_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &opti621_chipset);
+	return ide_pci_init_one(dev, &opti621_chipset, NULL);
 }
 
 static const struct pci_device_id opti621_pci_tbl[] = {
@@ -223,6 +225,7 @@ static struct pci_driver driver = {
 	.name		= "Opti621_IDE",
 	.id_table	= opti621_pci_tbl,
 	.probe		= opti621_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init opti621_ide_init(void)
@@ -230,7 +233,13 @@ static int __init opti621_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit opti621_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(opti621_ide_init);
+module_exit(opti621_ide_exit);
 
 MODULE_AUTHOR("Jaromir Koutek, Jan Harkes, Mark Lord");
 MODULE_DESCRIPTION("PCI driver module for Opti621 IDE");
diff --git a/drivers/ide/pci/pdc202xx_new.c b/drivers/ide/pci/pdc202xx_new.c
index 070df8ab3b2..998615fa285 100644
--- a/drivers/ide/pci/pdc202xx_new.c
+++ b/drivers/ide/pci/pdc202xx_new.c
@@ -31,6 +31,8 @@
 #include <asm/pci-bridge.h>
 #endif
 
+#define DRV_NAME "pdc202xx_new"
+
 #undef DEBUG
 
 #ifdef DEBUG
@@ -324,8 +326,9 @@ static void __devinit apple_kiwi_init(struct pci_dev *pdev)
 }
 #endif /* CONFIG_PPC_PMAC */
 
-static unsigned int __devinit init_chipset_pdcnew(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_pdcnew(struct pci_dev *dev)
 {
+	const char *name = DRV_NAME;
 	unsigned long dma_base = pci_resource_start(dev, 4);
 	unsigned long sec_dma_base = dma_base + 0x08;
 	long pll_input, pll_output, ratio;
@@ -358,12 +361,13 @@ static unsigned int __devinit init_chipset_pdcnew(struct pci_dev *dev, const cha
 	 * registers setting.
 	 */
 	pll_input = detect_pll_input_clock(dma_base);
-	printk("%s: PLL input clock is %ld kHz\n", name, pll_input / 1000);
+	printk(KERN_INFO "%s %s: PLL input clock is %ld kHz\n",
+		name, pci_name(dev), pll_input / 1000);
 
 	/* Sanity check */
 	if (unlikely(pll_input < 5000000L || pll_input > 70000000L)) {
-		printk(KERN_ERR "%s: Bad PLL input clock %ld Hz, giving up!\n",
-		       name, pll_input);
+		printk(KERN_ERR "%s %s: Bad PLL input clock %ld Hz, giving up!"
+			"\n", name, pci_name(dev), pll_input);
 		goto out;
 	}
 
@@ -399,7 +403,8 @@ static unsigned int __devinit init_chipset_pdcnew(struct pci_dev *dev, const cha
 		r = 0x00;
 	} else {
 		/* Invalid ratio */
-		printk(KERN_ERR "%s: Bad ratio %ld, giving up!\n", name, ratio);
+		printk(KERN_ERR "%s %s: Bad ratio %ld, giving up!\n",
+			name, pci_name(dev), ratio);
 		goto out;
 	}
 
@@ -409,7 +414,8 @@ static unsigned int __devinit init_chipset_pdcnew(struct pci_dev *dev, const cha
 
 	if (unlikely(f < 0 || f > 127)) {
 		/* Invalid F */
-		printk(KERN_ERR "%s: F[%d] invalid!\n", name, f);
+		printk(KERN_ERR "%s %s: F[%d] invalid!\n",
+			name, pci_name(dev), f);
 		goto out;
 	}
 
@@ -455,8 +461,8 @@ static struct pci_dev * __devinit pdc20270_get_dev2(struct pci_dev *dev)
 
 		if (dev2->irq != dev->irq) {
 			dev2->irq = dev->irq;
-			printk(KERN_INFO "PDC20270: PCI config space "
-					 "interrupt fixed\n");
+			printk(KERN_INFO DRV_NAME " %s: PCI config space "
+				"interrupt fixed\n", pci_name(dev));
 		}
 
 		return dev2;
@@ -473,9 +479,9 @@ static const struct ide_port_ops pdcnew_port_ops = {
 	.cable_detect		= pdcnew_cable_detect,
 };
 
-#define DECLARE_PDCNEW_DEV(name_str, udma) \
+#define DECLARE_PDCNEW_DEV(udma) \
 	{ \
-		.name		= name_str, \
+		.name		= DRV_NAME, \
 		.init_chipset	= init_chipset_pdcnew, \
 		.port_ops	= &pdcnew_port_ops, \
 		.host_flags	= IDE_HFLAG_POST_SET_MODE | \
@@ -487,13 +493,8 @@ static const struct ide_port_ops pdcnew_port_ops = {
 	}
 
 static const struct ide_port_info pdcnew_chipsets[] __devinitdata = {
-	/* 0 */ DECLARE_PDCNEW_DEV("PDC20268", ATA_UDMA5),
-	/* 1 */ DECLARE_PDCNEW_DEV("PDC20269", ATA_UDMA6),
-	/* 2 */ DECLARE_PDCNEW_DEV("PDC20270", ATA_UDMA5),
-	/* 3 */ DECLARE_PDCNEW_DEV("PDC20271", ATA_UDMA6),
-	/* 4 */ DECLARE_PDCNEW_DEV("PDC20275", ATA_UDMA6),
-	/* 5 */ DECLARE_PDCNEW_DEV("PDC20276", ATA_UDMA6),
-	/* 6 */ DECLARE_PDCNEW_DEV("PDC20277", ATA_UDMA6),
+	/* 0: PDC202{68,70} */		DECLARE_PDCNEW_DEV(ATA_UDMA5),
+	/* 1: PDC202{69,71,75,76,77} */	DECLARE_PDCNEW_DEV(ATA_UDMA6),
 };
 
 /**
@@ -507,13 +508,10 @@ static const struct ide_port_info pdcnew_chipsets[] __devinitdata = {
  
 static int __devinit pdc202new_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	const struct ide_port_info *d;
+	const struct ide_port_info *d = &pdcnew_chipsets[id->driver_data];
 	struct pci_dev *bridge = dev->bus->self;
-	u8 idx = id->driver_data;
-
-	d = &pdcnew_chipsets[idx];
 
-	if (idx == 2 && bridge &&
+	if (dev->device == PCI_DEVICE_ID_PROMISE_20270 && bridge &&
 	    bridge->vendor == PCI_VENDOR_ID_DEC &&
 	    bridge->device == PCI_DEVICE_ID_DEC_21150) {
 		struct pci_dev *dev2;
@@ -524,33 +522,42 @@ static int __devinit pdc202new_init_one(struct pci_dev *dev, const struct pci_de
 		dev2 = pdc20270_get_dev2(dev);
 
 		if (dev2) {
-			int ret = ide_setup_pci_devices(dev, dev2, d);
+			int ret = ide_pci_init_two(dev, dev2, d, NULL);
 			if (ret < 0)
 				pci_dev_put(dev2);
 			return ret;
 		}
 	}
 
-	if (idx == 5 && bridge &&
+	if (dev->device == PCI_DEVICE_ID_PROMISE_20276 && bridge &&
 	    bridge->vendor == PCI_VENDOR_ID_INTEL &&
 	    (bridge->device == PCI_DEVICE_ID_INTEL_I960 ||
 	     bridge->device == PCI_DEVICE_ID_INTEL_I960RM)) {
-		printk(KERN_INFO "PDC20276: attached to I2O RAID controller, "
-				 "skipping\n");
+		printk(KERN_INFO DRV_NAME " %s: attached to I2O RAID controller,"
+			" skipping\n", pci_name(dev));
 		return -ENODEV;
 	}
 
-	return ide_setup_pci_device(dev, d);
+	return ide_pci_init_one(dev, d, NULL);
+}
+
+static void __devexit pdc202new_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct pci_dev *dev2 = host->dev[1] ? to_pci_dev(host->dev[1]) : NULL;
+
+	ide_pci_remove(dev);
+	pci_dev_put(dev2);
 }
 
 static const struct pci_device_id pdc202new_pci_tbl[] = {
 	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20268), 0 },
 	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20269), 1 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20270), 2 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20271), 3 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20275), 4 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20276), 5 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20277), 6 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20270), 0 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20271), 1 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20275), 1 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20276), 1 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20277), 1 },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, pdc202new_pci_tbl);
@@ -559,6 +566,7 @@ static struct pci_driver driver = {
 	.name		= "Promise_IDE",
 	.id_table	= pdc202new_pci_tbl,
 	.probe		= pdc202new_init_one,
+	.remove		= pdc202new_remove,
 };
 
 static int __init pdc202new_ide_init(void)
@@ -566,7 +574,13 @@ static int __init pdc202new_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit pdc202new_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(pdc202new_ide_init);
+module_exit(pdc202new_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick, Frank Tiernan");
 MODULE_DESCRIPTION("PCI driver module for Promise PDC20268 and higher");
diff --git a/drivers/ide/pci/pdc202xx_old.c b/drivers/ide/pci/pdc202xx_old.c
index e54dc653b8c..6ff2def58da 100644
--- a/drivers/ide/pci/pdc202xx_old.c
+++ b/drivers/ide/pci/pdc202xx_old.c
@@ -20,6 +20,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "pdc202xx_old"
+
 #define PDC202XX_DEBUG_DRIVE_INFO	0
 
 static const char *pdc_quirk_drives[] = {
@@ -263,8 +265,7 @@ static void pdc202xx_dma_timeout(ide_drive_t *drive)
 	ide_dma_timeout(drive);
 }
 
-static unsigned int __devinit init_chipset_pdc202xx(struct pci_dev *dev,
-						    const char *name)
+static unsigned int __devinit init_chipset_pdc202xx(struct pci_dev *dev)
 {
 	unsigned long dmabase = pci_resource_start(dev, 4);
 	u8 udma_speed_flag = 0, primary_mode = 0, secondary_mode = 0;
@@ -304,8 +305,8 @@ static void __devinit pdc202ata4_fixup_irq(struct pci_dev *dev,
 		if (irq != irq2) {
 			pci_write_config_byte(dev,
 				(PCI_INTERRUPT_LINE)|0x80, irq);     /* 0xbc */
-			printk(KERN_INFO "%s: PCI config space interrupt "
-					 "mirror fixed\n", name);
+			printk(KERN_INFO "%s %s: PCI config space interrupt "
+				"mirror fixed\n", name, pci_name(dev));
 		}
 	}
 }
@@ -350,9 +351,9 @@ static const struct ide_dma_ops pdc2026x_dma_ops = {
 	.dma_timeout		= pdc202xx_dma_timeout,
 };
 
-#define DECLARE_PDC2026X_DEV(name_str, udma, extra_flags) \
+#define DECLARE_PDC2026X_DEV(udma, extra_flags) \
 	{ \
-		.name		= name_str, \
+		.name		= DRV_NAME, \
 		.init_chipset	= init_chipset_pdc202xx, \
 		.port_ops	= &pdc2026x_port_ops, \
 		.dma_ops	= &pdc2026x_dma_ops, \
@@ -363,8 +364,8 @@ static const struct ide_dma_ops pdc2026x_dma_ops = {
 	}
 
 static const struct ide_port_info pdc202xx_chipsets[] __devinitdata = {
-	{	/* 0 */
-		.name		= "PDC20246",
+	{	/* 0: PDC20246 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_pdc202xx,
 		.port_ops	= &pdc20246_port_ops,
 		.dma_ops	= &pdc20246_dma_ops,
@@ -374,10 +375,10 @@ static const struct ide_port_info pdc202xx_chipsets[] __devinitdata = {
 		.udma_mask	= ATA_UDMA2,
 	},
 
-	/* 1 */ DECLARE_PDC2026X_DEV("PDC20262", ATA_UDMA4, 0),
-	/* 2 */ DECLARE_PDC2026X_DEV("PDC20263", ATA_UDMA4, 0),
-	/* 3 */ DECLARE_PDC2026X_DEV("PDC20265", ATA_UDMA5, IDE_HFLAG_RQSIZE_256),
-	/* 4 */ DECLARE_PDC2026X_DEV("PDC20267", ATA_UDMA5, IDE_HFLAG_RQSIZE_256),
+	/* 1: PDC2026{2,3} */
+	DECLARE_PDC2026X_DEV(ATA_UDMA4, 0),
+	/* 2: PDC2026{5,7} */
+	DECLARE_PDC2026X_DEV(ATA_UDMA5, IDE_HFLAG_RQSIZE_256),
 };
 
 /**
@@ -396,31 +397,32 @@ static int __devinit pdc202xx_init_one(struct pci_dev *dev, const struct pci_dev
 
 	d = &pdc202xx_chipsets[idx];
 
-	if (idx < 3)
+	if (idx < 2)
 		pdc202ata4_fixup_irq(dev, d->name);
 
-	if (idx == 3) {
+	if (dev->vendor == PCI_DEVICE_ID_PROMISE_20265) {
 		struct pci_dev *bridge = dev->bus->self;
 
 		if (bridge &&
 		    bridge->vendor == PCI_VENDOR_ID_INTEL &&
 		    (bridge->device == PCI_DEVICE_ID_INTEL_I960 ||
 		     bridge->device == PCI_DEVICE_ID_INTEL_I960RM)) {
-			printk(KERN_INFO "ide: Skipping Promise PDC20265 "
-				"attached to I2O RAID controller\n");
+			printk(KERN_INFO DRV_NAME " %s: skipping Promise "
+				"PDC20265 attached to I2O RAID controller\n",
+				pci_name(dev));
 			return -ENODEV;
 		}
 	}
 
-	return ide_setup_pci_device(dev, d);
+	return ide_pci_init_one(dev, d, NULL);
 }
 
 static const struct pci_device_id pdc202xx_pci_tbl[] = {
 	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20246), 0 },
 	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20262), 1 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20263), 2 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20265), 3 },
-	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20267), 4 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20263), 1 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20265), 2 },
+	{ PCI_VDEVICE(PROMISE, PCI_DEVICE_ID_PROMISE_20267), 2 },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, pdc202xx_pci_tbl);
@@ -429,6 +431,7 @@ static struct pci_driver driver = {
 	.name		= "Promise_Old_IDE",
 	.id_table	= pdc202xx_pci_tbl,
 	.probe		= pdc202xx_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init pdc202xx_ide_init(void)
@@ -436,7 +439,13 @@ static int __init pdc202xx_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit pdc202xx_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(pdc202xx_ide_init);
+module_exit(pdc202xx_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick, Frank Tiernan");
 MODULE_DESCRIPTION("PCI driver module for older Promise IDE");
diff --git a/drivers/ide/pci/piix.c b/drivers/ide/pci/piix.c
index 0ce41b4ddda..7fc3022dcf6 100644
--- a/drivers/ide/pci/piix.c
+++ b/drivers/ide/pci/piix.c
@@ -54,6 +54,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "piix"
+
 static int no_piix_dma;
 
 /**
@@ -198,13 +200,12 @@ static void piix_set_dma_mode(ide_drive_t *drive, const u8 speed)
 /**
  *	init_chipset_ich	-	set up the ICH chipset
  *	@dev: PCI device to set up
- *	@name: Name of the device
  *
  *	Initialize the PCI device as required.  For the ICH this turns
  *	out to be nice and simple.
  */
 
-static unsigned int __devinit init_chipset_ich(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_ich(struct pci_dev *dev)
 {
 	u32 extra = 0;
 
@@ -314,9 +315,9 @@ static const struct ide_port_ops piix_port_ops = {
  #define IDE_HFLAGS_PIIX 0
 #endif
 
-#define DECLARE_PIIX_DEV(name_str, udma) \
+#define DECLARE_PIIX_DEV(udma) \
 	{						\
-		.name		= name_str,		\
+		.name		= DRV_NAME,		\
 		.init_hwif	= init_hwif_piix,	\
 		.enablebits	= {{0x41,0x80,0x80}, {0x43,0x80,0x80}}, \
 		.port_ops	= &piix_port_ops,	\
@@ -327,9 +328,9 @@ static const struct ide_port_ops piix_port_ops = {
 		.udma_mask	= udma,			\
 	}
 
-#define DECLARE_ICH_DEV(name_str, udma) \
+#define DECLARE_ICH_DEV(udma) \
 	{ \
-		.name		= name_str, \
+		.name		= DRV_NAME, \
 		.init_chipset	= init_chipset_ich, \
 		.init_hwif	= init_hwif_ich, \
 		.enablebits	= {{0x41,0x80,0x80}, {0x43,0x80,0x80}}, \
@@ -342,45 +343,31 @@ static const struct ide_port_ops piix_port_ops = {
 	}
 
 static const struct ide_port_info piix_pci_info[] __devinitdata = {
-	/*  0 */ DECLARE_PIIX_DEV("PIIXa",	0x00),	/* no udma */
-	/*  1 */ DECLARE_PIIX_DEV("PIIXb",	0x00),	/* no udma */
-
-	/*  2 */
+	/* 0: MPIIX */
 	{	/*
 		 * MPIIX actually has only a single IDE channel mapped to
 		 * the primary or secondary ports depending on the value
 		 * of the bit 14 of the IDETIM register at offset 0x6c
 		 */
-		.name		= "MPIIX",
+		.name		= DRV_NAME,
 		.enablebits	= {{0x6d,0xc0,0x80}, {0x6d,0xc0,0xc0}},
 		.host_flags	= IDE_HFLAG_ISA_PORTS | IDE_HFLAG_NO_DMA |
 				  IDE_HFLAGS_PIIX,
 		.pio_mask	= ATA_PIO4,
 		/* This is a painful system best to let it self tune for now */
 	},
-
-	/*  3 */ DECLARE_PIIX_DEV("PIIX3",	0x00),	/* no udma */
-	/*  4 */ DECLARE_PIIX_DEV("PIIX4",	ATA_UDMA2),
-	/*  5 */ DECLARE_ICH_DEV("ICH0",	ATA_UDMA2),
-	/*  6 */ DECLARE_PIIX_DEV("PIIX4",	ATA_UDMA2),
-	/*  7 */ DECLARE_ICH_DEV("ICH",		ATA_UDMA4),
-	/*  8 */ DECLARE_PIIX_DEV("PIIX4",	ATA_UDMA4),
-	/*  9 */ DECLARE_PIIX_DEV("PIIX4",	ATA_UDMA2),
-	/* 10 */ DECLARE_ICH_DEV("ICH2",	ATA_UDMA5),
-	/* 11 */ DECLARE_ICH_DEV("ICH2M",	ATA_UDMA5),
-	/* 12 */ DECLARE_ICH_DEV("ICH3M",	ATA_UDMA5),
-	/* 13 */ DECLARE_ICH_DEV("ICH3",	ATA_UDMA5),
-	/* 14 */ DECLARE_ICH_DEV("ICH4",	ATA_UDMA5),
-	/* 15 */ DECLARE_ICH_DEV("ICH5",	ATA_UDMA5),
-	/* 16 */ DECLARE_ICH_DEV("C-ICH",	ATA_UDMA5),
-	/* 17 */ DECLARE_ICH_DEV("ICH4",	ATA_UDMA5),
-	/* 18 */ DECLARE_ICH_DEV("ICH5-SATA",	ATA_UDMA5),
-	/* 19 */ DECLARE_ICH_DEV("ICH5",	ATA_UDMA5),
-	/* 20 */ DECLARE_ICH_DEV("ICH6",	ATA_UDMA5),
-	/* 21 */ DECLARE_ICH_DEV("ICH7",	ATA_UDMA5),
-	/* 22 */ DECLARE_ICH_DEV("ICH4",	ATA_UDMA5),
-	/* 23 */ DECLARE_ICH_DEV("ESB2",	ATA_UDMA5),
-	/* 24 */ DECLARE_ICH_DEV("ICH8M",	ATA_UDMA5),
+	/* 1: PIIXa/PIIXb/PIIX3 */
+	DECLARE_PIIX_DEV(0x00), /* no udma */
+	/* 2: PIIX4 */
+	DECLARE_PIIX_DEV(ATA_UDMA2),
+	/* 3: ICH0 */
+	DECLARE_ICH_DEV(ATA_UDMA2),
+	/* 4: ICH */
+	DECLARE_ICH_DEV(ATA_UDMA4),
+	/* 5: PIIX4 */
+	DECLARE_PIIX_DEV(ATA_UDMA4),
+	/* 6: ICH[2-7]/ICH[2-3]M/C-ICH/ICH5-SATA/ESB2/ICH8M */
+	DECLARE_ICH_DEV(ATA_UDMA5),
 };
 
 /**
@@ -394,7 +381,7 @@ static const struct ide_port_info piix_pci_info[] __devinitdata = {
  
 static int __devinit piix_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &piix_pci_info[id->driver_data]);
+	return ide_pci_init_one(dev, &piix_pci_info[id->driver_data], NULL);
 }
 
 /**
@@ -421,39 +408,39 @@ static void __devinit piix_check_450nx(void)
 			no_piix_dma = 2;
 	}
 	if(no_piix_dma)
-		printk(KERN_WARNING "piix: 450NX errata present, disabling IDE DMA.\n");
+		printk(KERN_WARNING DRV_NAME ": 450NX errata present, disabling IDE DMA.\n");
 	if(no_piix_dma == 2)
-		printk(KERN_WARNING "piix: A BIOS update may resolve this.\n");
+		printk(KERN_WARNING DRV_NAME ": A BIOS update may resolve this.\n");
 }		
 
 static const struct pci_device_id piix_pci_tbl[] = {
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371FB_0),   0 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371FB_1),   1 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371MX),     2 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371SB_1),   3 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371AB),     4 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801AB_1),   5 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82443MX_1),   6 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801AA_1),   7 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82372FB_1),   8 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82451NX),     9 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801BA_9),  10 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801BA_8),  11 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801CA_10), 12 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801CA_11), 13 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_11), 14 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801EB_11), 15 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801E_11),  16 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_10), 17 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371FB_0),  1 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371FB_1),  1 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371MX),    0 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371SB_1),  1 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82371AB),    2 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801AB_1),  3 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82443MX_1),  2 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801AA_1),  4 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82372FB_1),  5 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82451NX),    2 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801BA_9),  6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801BA_8),  6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801CA_10), 6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801CA_11), 6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_11), 6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801EB_11), 6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801E_11),  6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_10), 6 },
 #ifdef CONFIG_BLK_DEV_IDE_SATA
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801EB_1),  18 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801EB_1),  6 },
 #endif
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ESB_2),      19 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH6_19),    20 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH7_21),    21 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_1),  22 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ESB2_18),    23 },
-	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH8_6),     24 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ESB_2),      6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH6_19),    6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH7_21),    6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_82801DB_1),  6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ESB2_18),    6 },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ICH8_6),     6 },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, piix_pci_tbl);
@@ -462,6 +449,7 @@ static struct pci_driver driver = {
 	.name		= "PIIX_IDE",
 	.id_table	= piix_pci_tbl,
 	.probe		= piix_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init piix_ide_init(void)
@@ -470,7 +458,13 @@ static int __init piix_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit piix_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(piix_ide_init);
+module_exit(piix_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick, Andrzej Krzysztofowicz");
 MODULE_DESCRIPTION("PCI driver module for Intel PIIX IDE");
diff --git a/drivers/ide/pci/rz1000.c b/drivers/ide/pci/rz1000.c
index 532154adba2..8d11ee838a2 100644
--- a/drivers/ide/pci/rz1000.c
+++ b/drivers/ide/pci/rz1000.c
@@ -21,6 +21,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "rz1000"
+
 static void __devinit init_hwif_rz1000 (ide_hwif_t *hwif)
 {
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
@@ -40,7 +42,7 @@ static void __devinit init_hwif_rz1000 (ide_hwif_t *hwif)
 }
 
 static const struct ide_port_info rz1000_chipset __devinitdata = {
-	.name		= "RZ100x",
+	.name		= DRV_NAME,
 	.init_hwif	= init_hwif_rz1000,
 	.chipset	= ide_rz1000,
 	.host_flags	= IDE_HFLAG_NO_DMA,
@@ -48,7 +50,7 @@ static const struct ide_port_info rz1000_chipset __devinitdata = {
 
 static int __devinit rz1000_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &rz1000_chipset);
+	return ide_pci_init_one(dev, &rz1000_chipset, NULL);
 }
 
 static const struct pci_device_id rz1000_pci_tbl[] = {
@@ -62,6 +64,7 @@ static struct pci_driver driver = {
 	.name		= "RZ1000_IDE",
 	.id_table	= rz1000_pci_tbl,
 	.probe		= rz1000_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init rz1000_ide_init(void)
@@ -69,7 +72,13 @@ static int __init rz1000_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit rz1000_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(rz1000_ide_init);
+module_exit(rz1000_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for RZ1000 IDE");
diff --git a/drivers/ide/pci/sc1200.c b/drivers/ide/pci/sc1200.c
index 14c787b5d95..8efaed16fea 100644
--- a/drivers/ide/pci/sc1200.c
+++ b/drivers/ide/pci/sc1200.c
@@ -22,6 +22,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "sc1200"
+
 #define SC1200_REV_A	0x00
 #define SC1200_REV_B1	0x01
 #define SC1200_REV_B3	0x02
@@ -234,21 +236,11 @@ static int sc1200_suspend (struct pci_dev *dev, pm_message_t state)
 	 * we only save state when going from full power to less
 	 */
 	if (state.event == PM_EVENT_ON) {
-		struct sc1200_saved_state *ss;
+		struct ide_host *host = pci_get_drvdata(dev);
+		struct sc1200_saved_state *ss = host->host_priv;
 		unsigned int r;
 
 		/*
-		 * allocate a permanent save area, if not already allocated
-		 */
-		ss = (struct sc1200_saved_state *)pci_get_drvdata(dev);
-		if (ss == NULL) {
-			ss = kmalloc(sizeof(*ss), GFP_KERNEL);
-			if (ss == NULL)
-				return -ENOMEM;
-			pci_set_drvdata(dev, ss);
-		}
-
-		/*
 		 * save timing registers
 		 * (this may be unnecessary if BIOS also does it)
 		 */
@@ -263,7 +255,8 @@ static int sc1200_suspend (struct pci_dev *dev, pm_message_t state)
 
 static int sc1200_resume (struct pci_dev *dev)
 {
-	struct sc1200_saved_state *ss;
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct sc1200_saved_state *ss = host->host_priv;
 	unsigned int r;
 	int i;
 
@@ -271,16 +264,12 @@ static int sc1200_resume (struct pci_dev *dev)
 	if (i)
 		return i;
 
-	ss = (struct sc1200_saved_state *)pci_get_drvdata(dev);
-
 	/*
 	 * restore timing registers
 	 * (this may be unnecessary if BIOS also does it)
 	 */
-	if (ss) {
-		for (r = 0; r < 8; r++)
-			pci_write_config_dword(dev, 0x40 + r * 4, ss->regs[r]);
-	}
+	for (r = 0; r < 8; r++)
+		pci_write_config_dword(dev, 0x40 + r * 4, ss->regs[r]);
 
 	return 0;
 }
@@ -304,7 +293,7 @@ static const struct ide_dma_ops sc1200_dma_ops = {
 };
 
 static const struct ide_port_info sc1200_chipset __devinitdata = {
-	.name		= "SC1200",
+	.name		= DRV_NAME,
 	.port_ops	= &sc1200_port_ops,
 	.dma_ops	= &sc1200_dma_ops,
 	.host_flags	= IDE_HFLAG_SERIALIZE |
@@ -317,7 +306,19 @@ static const struct ide_port_info sc1200_chipset __devinitdata = {
 
 static int __devinit sc1200_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &sc1200_chipset);
+	struct sc1200_saved_state *ss = NULL;
+	int rc;
+
+#ifdef CONFIG_PM
+	ss = kmalloc(sizeof(*ss), GFP_KERNEL);
+	if (ss == NULL)
+		return -ENOMEM;
+#endif
+	rc = ide_pci_init_one(dev, &sc1200_chipset, ss);
+	if (rc)
+		kfree(ss);
+
+	return rc;
 }
 
 static const struct pci_device_id sc1200_pci_tbl[] = {
@@ -330,6 +331,7 @@ static struct pci_driver driver = {
 	.name		= "SC1200_IDE",
 	.id_table	= sc1200_pci_tbl,
 	.probe		= sc1200_init_one,
+	.remove		= ide_pci_remove,
 #ifdef CONFIG_PM
 	.suspend	= sc1200_suspend,
 	.resume		= sc1200_resume,
@@ -341,7 +343,13 @@ static int __init sc1200_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit sc1200_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(sc1200_ide_init);
+module_exit(sc1200_ide_exit);
 
 MODULE_AUTHOR("Mark Lord");
 MODULE_DESCRIPTION("PCI driver module for NS SC1200 IDE");
diff --git a/drivers/ide/pci/serverworks.c b/drivers/ide/pci/serverworks.c
index 127ccb45e26..d173f293772 100644
--- a/drivers/ide/pci/serverworks.c
+++ b/drivers/ide/pci/serverworks.c
@@ -38,6 +38,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "serverworks"
+
 #define SVWKS_CSB5_REVISION_NEW	0x92 /* min PCI_REVISION_ID for UDMA5 (A2.0) */
 #define SVWKS_CSB6_REVISION	0xa0 /* min PCI_REVISION_ID for UDMA4 (A1.0) */
 
@@ -172,7 +174,7 @@ static void svwks_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	pci_write_config_byte(dev, 0x54, ultra_enable);
 }
 
-static unsigned int __devinit init_chipset_svwks (struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_svwks(struct pci_dev *dev)
 {
 	unsigned int reg;
 	u8 btr;
@@ -188,7 +190,8 @@ static unsigned int __devinit init_chipset_svwks (struct pci_dev *dev, const cha
 			pci_read_config_dword(isa_dev, 0x64, &reg);
 			reg &= ~0x00002000; /* disable 600ns interrupt mask */
 			if(!(reg & 0x00004000))
-				printk(KERN_DEBUG "%s: UDMA not BIOS enabled.\n", name);
+				printk(KERN_DEBUG DRV_NAME " %s: UDMA not BIOS "
+					"enabled.\n", pci_name(dev));
 			reg |=  0x00004000; /* enable UDMA/33 support */
 			pci_write_config_dword(isa_dev, 0x64, reg);
 		}
@@ -352,40 +355,44 @@ static const struct ide_port_ops svwks_port_ops = {
 #define IDE_HFLAGS_SVWKS IDE_HFLAG_LEGACY_IRQS
 
 static const struct ide_port_info serverworks_chipsets[] __devinitdata = {
-	{	/* 0 */
-		.name		= "SvrWks OSB4",
+	{	/* 0: OSB4 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_svwks,
 		.port_ops	= &osb4_port_ops,
 		.host_flags	= IDE_HFLAGS_SVWKS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= 0x00, /* UDMA is problematic on OSB4 */
-	},{	/* 1 */
-		.name		= "SvrWks CSB5",
+	},
+	{	/* 1: CSB5 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_svwks,
 		.port_ops	= &svwks_port_ops,
 		.host_flags	= IDE_HFLAGS_SVWKS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
-	},{	/* 2 */
-		.name		= "SvrWks CSB6",
+	},
+	{	/* 2: CSB6 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_svwks,
 		.port_ops	= &svwks_port_ops,
 		.host_flags	= IDE_HFLAGS_SVWKS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
-	},{	/* 3 */
-		.name		= "SvrWks CSB6",
+	},
+	{	/* 3: CSB6-2 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_svwks,
 		.port_ops	= &svwks_port_ops,
 		.host_flags	= IDE_HFLAGS_SVWKS | IDE_HFLAG_SINGLE,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
-	},{	/* 4 */
-		.name		= "SvrWks HT1000",
+	},
+	{	/* 4: HT1000 */
+		.name		= DRV_NAME,
 		.init_chipset	= init_chipset_svwks,
 		.port_ops	= &svwks_port_ops,
 		.host_flags	= IDE_HFLAGS_SVWKS | IDE_HFLAG_SINGLE,
@@ -422,7 +429,7 @@ static int __devinit svwks_init_one(struct pci_dev *dev, const struct pci_device
 			d.host_flags &= ~IDE_HFLAG_SINGLE;
 	}
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 static const struct pci_device_id svwks_pci_tbl[] = {
@@ -439,6 +446,7 @@ static struct pci_driver driver = {
 	.name		= "Serverworks_IDE",
 	.id_table	= svwks_pci_tbl,
 	.probe		= svwks_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init svwks_ide_init(void)
@@ -446,7 +454,13 @@ static int __init svwks_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit svwks_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(svwks_ide_init);
+module_exit(svwks_ide_exit);
 
 MODULE_AUTHOR("Michael Aubry. Andrzej Krzysztofowicz, Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for Serverworks OSB4/CSB5/CSB6 IDE");
diff --git a/drivers/ide/pci/siimage.c b/drivers/ide/pci/siimage.c
index 5965a35d94a..b8ad9ad6cf0 100644
--- a/drivers/ide/pci/siimage.c
+++ b/drivers/ide/pci/siimage.c
@@ -44,6 +44,8 @@
 #include <linux/init.h>
 #include <linux/io.h>
 
+#define DRV_NAME "siimage"
+
 /**
  *	pdev_is_sata		-	check if device is SATA
  *	@pdev:	PCI device to check
@@ -127,9 +129,10 @@ static inline unsigned long siimage_seldev(ide_drive_t *drive, int r)
 
 static u8 sil_ioread8(struct pci_dev *dev, unsigned long addr)
 {
+	struct ide_host *host = pci_get_drvdata(dev);
 	u8 tmp = 0;
 
-	if (pci_get_drvdata(dev))
+	if (host->host_priv)
 		tmp = readb((void __iomem *)addr);
 	else
 		pci_read_config_byte(dev, addr, &tmp);
@@ -139,9 +142,10 @@ static u8 sil_ioread8(struct pci_dev *dev, unsigned long addr)
 
 static u16 sil_ioread16(struct pci_dev *dev, unsigned long addr)
 {
+	struct ide_host *host = pci_get_drvdata(dev);
 	u16 tmp = 0;
 
-	if (pci_get_drvdata(dev))
+	if (host->host_priv)
 		tmp = readw((void __iomem *)addr);
 	else
 		pci_read_config_word(dev, addr, &tmp);
@@ -151,7 +155,9 @@ static u16 sil_ioread16(struct pci_dev *dev, unsigned long addr)
 
 static void sil_iowrite8(struct pci_dev *dev, u8 val, unsigned long addr)
 {
-	if (pci_get_drvdata(dev))
+	struct ide_host *host = pci_get_drvdata(dev);
+
+	if (host->host_priv)
 		writeb(val, (void __iomem *)addr);
 	else
 		pci_write_config_byte(dev, addr, val);
@@ -159,7 +165,9 @@ static void sil_iowrite8(struct pci_dev *dev, u8 val, unsigned long addr)
 
 static void sil_iowrite16(struct pci_dev *dev, u16 val, unsigned long addr)
 {
-	if (pci_get_drvdata(dev))
+	struct ide_host *host = pci_get_drvdata(dev);
+
+	if (host->host_priv)
 		writew(val, (void __iomem *)addr);
 	else
 		pci_write_config_word(dev, addr, val);
@@ -167,7 +175,9 @@ static void sil_iowrite16(struct pci_dev *dev, u16 val, unsigned long addr)
 
 static void sil_iowrite32(struct pci_dev *dev, u32 val, unsigned long addr)
 {
-	if (pci_get_drvdata(dev))
+	struct ide_host *host = pci_get_drvdata(dev);
+
+	if (host->host_priv)
 		writel(val, (void __iomem *)addr);
 	else
 		pci_write_config_dword(dev, addr, val);
@@ -445,66 +455,24 @@ static void sil_sata_pre_reset(ide_drive_t *drive)
 }
 
 /**
- *	setup_mmio_siimage	-	switch controller into MMIO mode
- *	@dev: PCI device we are configuring
- *	@name: device name
- *
- *	Attempt to put the device into MMIO mode. There are some slight
- *	complications here with certain systems where the MMIO BAR isn't
- *	mapped, so we have to be sure that we can fall back to I/O.
- */
-
-static unsigned int setup_mmio_siimage(struct pci_dev *dev, const char *name)
-{
-	resource_size_t bar5	= pci_resource_start(dev, 5);
-	unsigned long barsize	= pci_resource_len(dev, 5);
-	void __iomem *ioaddr;
-
-	/*
-	 *	Drop back to PIO if we can't map the MMIO. Some	systems
-	 *	seem to get terminally confused in the PCI spaces.
-	 */
-	if (!request_mem_region(bar5, barsize, name)) {
-		printk(KERN_WARNING "siimage: IDE controller MMIO ports not "
-				    "available.\n");
-		return 0;
-	}
-
-	ioaddr = ioremap(bar5, barsize);
-	if (ioaddr == NULL) {
-		release_mem_region(bar5, barsize);
-		return 0;
-	}
-
-	pci_set_master(dev);
-	pci_set_drvdata(dev, (void *) ioaddr);
-
-	return 1;
-}
-
-/**
  *	init_chipset_siimage	-	set up an SI device
  *	@dev: PCI device
- *	@name: device name
  *
  *	Perform the initial PCI set up for this device. Attempt to switch
  *	to 133 MHz clocking if the system isn't already set up to do it.
  */
 
-static unsigned int __devinit init_chipset_siimage(struct pci_dev *dev,
-						   const char *name)
+static unsigned int __devinit init_chipset_siimage(struct pci_dev *dev)
 {
+	struct ide_host *host = pci_get_drvdata(dev);
+	void __iomem *ioaddr = host->host_priv;
 	unsigned long base, scsc_addr;
-	void __iomem *ioaddr = NULL;
-	u8 rev = dev->revision, tmp, BA5_EN;
+	u8 rev = dev->revision, tmp;
 
 	pci_write_config_byte(dev, PCI_CACHE_LINE_SIZE, rev ? 1 : 255);
 
-	pci_read_config_byte(dev, 0x8A, &BA5_EN);
-
-	if ((BA5_EN & 0x01) || pci_resource_start(dev, 5))
-		if (setup_mmio_siimage(dev, name))
-			ioaddr = pci_get_drvdata(dev);
+	if (ioaddr)
+		pci_set_master(dev);
 
 	base = (unsigned long)ioaddr;
 
@@ -571,7 +539,8 @@ static unsigned int __devinit init_chipset_siimage(struct pci_dev *dev,
 			{ "== 100", "== 133", "== 2X PCI", "DISABLED!" };
 
 		tmp >>= 4;
-		printk(KERN_INFO "%s: BASE CLOCK %s\n", name, clk_str[tmp & 3]);
+		printk(KERN_INFO DRV_NAME " %s: BASE CLOCK %s\n",
+			pci_name(dev), clk_str[tmp & 3]);
 	}
 
 	return 0;
@@ -592,7 +561,8 @@ static unsigned int __devinit init_chipset_siimage(struct pci_dev *dev,
 static void __devinit init_mmio_iops_siimage(ide_hwif_t *hwif)
 {
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
-	void *addr		= pci_get_drvdata(dev);
+	struct ide_host *host	= pci_get_drvdata(dev);
+	void *addr		= host->host_priv;
 	u8 ch			= hwif->channel;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	unsigned long base;
@@ -691,16 +661,15 @@ static void __devinit sil_quirkproc(ide_drive_t *drive)
 static void __devinit init_iops_siimage(ide_hwif_t *hwif)
 {
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
+	struct ide_host *host = pci_get_drvdata(dev);
 
 	hwif->hwif_data = NULL;
 
 	/* Pessimal until we finish probing */
 	hwif->rqsize = 15;
 
-	if (pci_get_drvdata(dev) == NULL)
-		return;
-
-	init_mmio_iops_siimage(hwif);
+	if (host->host_priv)
+		init_mmio_iops_siimage(hwif);
 }
 
 /**
@@ -748,9 +717,9 @@ static const struct ide_dma_ops sil_dma_ops = {
 	.dma_lost_irq		= ide_dma_lost_irq,
 };
 
-#define DECLARE_SII_DEV(name_str, p_ops)		\
+#define DECLARE_SII_DEV(p_ops)				\
 	{						\
-		.name		= name_str,		\
+		.name		= DRV_NAME,		\
 		.init_chipset	= init_chipset_siimage,	\
 		.init_iops	= init_iops_siimage,	\
 		.port_ops	= p_ops,		\
@@ -761,9 +730,8 @@ static const struct ide_dma_ops sil_dma_ops = {
 	}
 
 static const struct ide_port_info siimage_chipsets[] __devinitdata = {
-	/* 0 */ DECLARE_SII_DEV("SiI680",		&sil_pata_port_ops),
-	/* 1 */ DECLARE_SII_DEV("SiI3112 Serial ATA",	&sil_sata_port_ops),
-	/* 2 */ DECLARE_SII_DEV("Adaptec AAR-1210SA",	&sil_sata_port_ops)
+	/* 0: SiI680 */  DECLARE_SII_DEV(&sil_pata_port_ops),
+	/* 1: SiI3112 */ DECLARE_SII_DEV(&sil_sata_port_ops)
 };
 
 /**
@@ -778,8 +746,13 @@ static const struct ide_port_info siimage_chipsets[] __devinitdata = {
 static int __devinit siimage_init_one(struct pci_dev *dev,
 				      const struct pci_device_id *id)
 {
+	void __iomem *ioaddr = NULL;
+	resource_size_t bar5 = pci_resource_start(dev, 5);
+	unsigned long barsize = pci_resource_len(dev, 5);
+	int rc;
 	struct ide_port_info d;
 	u8 idx = id->driver_data;
+	u8 BA5_EN;
 
 	d = siimage_chipsets[idx];
 
@@ -787,7 +760,7 @@ static int __devinit siimage_init_one(struct pci_dev *dev,
 		static int first = 1;
 
 		if (first) {
-			printk(KERN_INFO "siimage: For full SATA support you "
+			printk(KERN_INFO DRV_NAME ": For full SATA support you "
 				"should use the libata sata_sil module.\n");
 			first = 0;
 		}
@@ -795,14 +768,61 @@ static int __devinit siimage_init_one(struct pci_dev *dev,
 		d.host_flags |= IDE_HFLAG_NO_ATAPI_DMA;
 	}
 
-	return ide_setup_pci_device(dev, &d);
+	rc = pci_enable_device(dev);
+	if (rc)
+		return rc;
+
+	pci_read_config_byte(dev, 0x8A, &BA5_EN);
+	if ((BA5_EN & 0x01) || bar5) {
+		/*
+		* Drop back to PIO if we can't map the MMIO. Some systems
+		* seem to get terminally confused in the PCI spaces.
+		*/
+		if (!request_mem_region(bar5, barsize, d.name)) {
+			printk(KERN_WARNING DRV_NAME " %s: MMIO ports not "
+				"available\n", pci_name(dev));
+		} else {
+			ioaddr = ioremap(bar5, barsize);
+			if (ioaddr == NULL)
+				release_mem_region(bar5, barsize);
+		}
+	}
+
+	rc = ide_pci_init_one(dev, &d, ioaddr);
+	if (rc) {
+		if (ioaddr) {
+			iounmap(ioaddr);
+			release_mem_region(bar5, barsize);
+		}
+		pci_disable_device(dev);
+	}
+
+	return rc;
+}
+
+static void __devexit siimage_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	void __iomem *ioaddr = host->host_priv;
+
+	ide_pci_remove(dev);
+
+	if (ioaddr) {
+		resource_size_t bar5 = pci_resource_start(dev, 5);
+		unsigned long barsize = pci_resource_len(dev, 5);
+
+		iounmap(ioaddr);
+		release_mem_region(bar5, barsize);
+	}
+
+	pci_disable_device(dev);
 }
 
 static const struct pci_device_id siimage_pci_tbl[] = {
 	{ PCI_VDEVICE(CMD, PCI_DEVICE_ID_SII_680),    0 },
 #ifdef CONFIG_BLK_DEV_IDE_SATA
 	{ PCI_VDEVICE(CMD, PCI_DEVICE_ID_SII_3112),   1 },
-	{ PCI_VDEVICE(CMD, PCI_DEVICE_ID_SII_1210SA), 2 },
+	{ PCI_VDEVICE(CMD, PCI_DEVICE_ID_SII_1210SA), 1 },
 #endif
 	{ 0, },
 };
@@ -812,6 +832,7 @@ static struct pci_driver driver = {
 	.name		= "SiI_IDE",
 	.id_table	= siimage_pci_tbl,
 	.probe		= siimage_init_one,
+	.remove		= siimage_remove,
 };
 
 static int __init siimage_ide_init(void)
@@ -819,7 +840,13 @@ static int __init siimage_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit siimage_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(siimage_ide_init);
+module_exit(siimage_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick, Alan Cox");
 MODULE_DESCRIPTION("PCI driver module for SiI IDE");
diff --git a/drivers/ide/pci/sis5513.c b/drivers/ide/pci/sis5513.c
index 2389945ca95..cc95f90b53b 100644
--- a/drivers/ide/pci/sis5513.c
+++ b/drivers/ide/pci/sis5513.c
@@ -52,6 +52,8 @@
 #include <linux/init.h>
 #include <linux/ide.h>
 
+#define DRV_NAME "sis5513"
+
 /* registers layout and init values are chipset family dependant */
 
 #define ATA_16		0x01
@@ -380,8 +382,9 @@ static int __devinit sis_find_family(struct pci_dev *dev)
 		}
 		pci_dev_put(host);
 
-		printk(KERN_INFO "SIS5513: %s %s controller\n",
-			 SiSHostChipInfo[i].name, chipset_capability[chipset_family]);
+		printk(KERN_INFO DRV_NAME " %s: %s %s controller\n",
+			pci_name(dev), SiSHostChipInfo[i].name,
+			chipset_capability[chipset_family]);
 	}
 
 	if (!chipset_family) { /* Belongs to pci-quirks */
@@ -396,7 +399,8 @@ static int __devinit sis_find_family(struct pci_dev *dev)
 			pci_write_config_dword(dev, 0x54, idemisc);
 
 			if (trueid == 0x5518) {
-				printk(KERN_INFO "SIS5513: SiS 962/963 MuTIOL IDE UDMA133 controller\n");
+				printk(KERN_INFO DRV_NAME " %s: SiS 962/963 MuTIOL IDE UDMA133 controller\n",
+					pci_name(dev));
 				chipset_family = ATA_133;
 
 				/* Check for 5513 compability mapping
@@ -405,7 +409,8 @@ static int __devinit sis_find_family(struct pci_dev *dev)
 				 */
 				if ((idemisc & 0x40000000) == 0) {
 					pci_write_config_dword(dev, 0x54, idemisc | 0x40000000);
-					printk(KERN_INFO "SIS5513: Switching to 5513 register mapping\n");
+					printk(KERN_INFO DRV_NAME " %s: Switching to 5513 register mapping\n",
+						pci_name(dev));
 				}
 			}
 	}
@@ -429,10 +434,12 @@ static int __devinit sis_find_family(struct pci_dev *dev)
 				pci_dev_put(lpc_bridge);
 
 				if (lpc_bridge->revision == 0x10 && (prefctl & 0x80)) {
-					printk(KERN_INFO "SIS5513: SiS 961B MuTIOL IDE UDMA133 controller\n");
+					printk(KERN_INFO DRV_NAME " %s: SiS 961B MuTIOL IDE UDMA133 controller\n",
+						pci_name(dev));
 					chipset_family = ATA_133a;
 				} else {
-					printk(KERN_INFO "SIS5513: SiS 961 MuTIOL IDE UDMA100 controller\n");
+					printk(KERN_INFO DRV_NAME " %s: SiS 961 MuTIOL IDE UDMA100 controller\n",
+						pci_name(dev));
 					chipset_family = ATA_100;
 				}
 			}
@@ -441,8 +448,7 @@ static int __devinit sis_find_family(struct pci_dev *dev)
 	return chipset_family;
 }
 
-static unsigned int __devinit init_chipset_sis5513(struct pci_dev *dev,
-						   const char *name)
+static unsigned int __devinit init_chipset_sis5513(struct pci_dev *dev)
 {
 	/* Make general config ops here
 	   1/ tell IDE channels to operate in Compatibility mode only
@@ -555,7 +561,7 @@ static const struct ide_port_ops sis_ata133_port_ops = {
 };
 
 static const struct ide_port_info sis5513_chipset __devinitdata = {
-	.name		= "SIS5513",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_sis5513,
 	.enablebits	= { {0x4a, 0x02, 0x02}, {0x4a, 0x04, 0x04} },
 	.host_flags	= IDE_HFLAG_LEGACY_IRQS | IDE_HFLAG_NO_AUTODMA,
@@ -583,7 +589,13 @@ static int __devinit sis5513_init_one(struct pci_dev *dev, const struct pci_devi
 
 	d.udma_mask = udma_rates[chipset_family];
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
+}
+
+static void __devexit sis5513_remove(struct pci_dev *dev)
+{
+	ide_pci_remove(dev);
+	pci_disable_device(dev);
 }
 
 static const struct pci_device_id sis5513_pci_tbl[] = {
@@ -598,6 +610,7 @@ static struct pci_driver driver = {
 	.name		= "SIS_IDE",
 	.id_table	= sis5513_pci_tbl,
 	.probe		= sis5513_init_one,
+	.remove		= sis5513_remove,
 };
 
 static int __init sis5513_ide_init(void)
@@ -605,7 +618,13 @@ static int __init sis5513_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit sis5513_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(sis5513_ide_init);
+module_exit(sis5513_ide_exit);
 
 MODULE_AUTHOR("Lionel Bouton, L C Chang, Andre Hedrick, Vojtech Pavlik");
 MODULE_DESCRIPTION("PCI driver module for SIS IDE");
diff --git a/drivers/ide/pci/sl82c105.c b/drivers/ide/pci/sl82c105.c
index f82a6502c1b..73905bcc08f 100644
--- a/drivers/ide/pci/sl82c105.c
+++ b/drivers/ide/pci/sl82c105.c
@@ -23,6 +23,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "sl82c105"
+
 #undef DEBUG
 
 #ifdef DEBUG
@@ -270,7 +272,7 @@ static u8 sl82c105_bridge_revision(struct pci_dev *dev)
  * channel 0 here at least, but channel 1 has to be enabled by
  * firmware or arch code. We still set both to 16 bits mode.
  */
-static unsigned int __devinit init_chipset_sl82c105(struct pci_dev *dev, const char *msg)
+static unsigned int __devinit init_chipset_sl82c105(struct pci_dev *dev)
 {
 	u32 val;
 
@@ -301,7 +303,7 @@ static const struct ide_dma_ops sl82c105_dma_ops = {
 };
 
 static const struct ide_port_info sl82c105_chipset __devinitdata = {
-	.name		= "W82C105",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_sl82c105,
 	.enablebits	= {{0x40,0x01,0x01}, {0x40,0x10,0x10}},
 	.port_ops	= &sl82c105_port_ops,
@@ -328,14 +330,14 @@ static int __devinit sl82c105_init_one(struct pci_dev *dev, const struct pci_dev
 		 * Never ever EVER under any circumstances enable
 		 * DMA when the bridge is this old.
 		 */
-		printk(KERN_INFO "W82C105_IDE: Winbond W83C553 bridge "
+		printk(KERN_INFO DRV_NAME ": Winbond W83C553 bridge "
 				 "revision %d, BM-DMA disabled\n", rev);
 		d.dma_ops = NULL;
 		d.mwdma_mask = 0;
 		d.host_flags &= ~IDE_HFLAG_SERIALIZE_DMA;
 	}
 
-	return ide_setup_pci_device(dev, &d);
+	return ide_pci_init_one(dev, &d, NULL);
 }
 
 static const struct pci_device_id sl82c105_pci_tbl[] = {
@@ -348,6 +350,7 @@ static struct pci_driver driver = {
 	.name		= "W82C105_IDE",
 	.id_table	= sl82c105_pci_tbl,
 	.probe		= sl82c105_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init sl82c105_ide_init(void)
@@ -355,7 +358,13 @@ static int __init sl82c105_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit sl82c105_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(sl82c105_ide_init);
+module_exit(sl82c105_ide_exit);
 
 MODULE_DESCRIPTION("PCI driver module for W82C105 IDE");
 MODULE_LICENSE("GPL");
diff --git a/drivers/ide/pci/slc90e66.c b/drivers/ide/pci/slc90e66.c
index dae6e2c94d8..13d1fa491f2 100644
--- a/drivers/ide/pci/slc90e66.c
+++ b/drivers/ide/pci/slc90e66.c
@@ -15,6 +15,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "slc90e66"
+
 static DEFINE_SPINLOCK(slc90e66_lock);
 
 static void slc90e66_set_pio_mode(ide_drive_t *drive, const u8 pio)
@@ -132,7 +134,7 @@ static const struct ide_port_ops slc90e66_port_ops = {
 };
 
 static const struct ide_port_info slc90e66_chipset __devinitdata = {
-	.name		= "SLC90E66",
+	.name		= DRV_NAME,
 	.enablebits	= { {0x41, 0x80, 0x80}, {0x43, 0x80, 0x80} },
 	.port_ops	= &slc90e66_port_ops,
 	.host_flags	= IDE_HFLAG_LEGACY_IRQS,
@@ -144,7 +146,7 @@ static const struct ide_port_info slc90e66_chipset __devinitdata = {
 
 static int __devinit slc90e66_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &slc90e66_chipset);
+	return ide_pci_init_one(dev, &slc90e66_chipset, NULL);
 }
 
 static const struct pci_device_id slc90e66_pci_tbl[] = {
@@ -157,6 +159,7 @@ static struct pci_driver driver = {
 	.name		= "SLC90e66_IDE",
 	.id_table	= slc90e66_pci_tbl,
 	.probe		= slc90e66_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init slc90e66_ide_init(void)
@@ -164,7 +167,13 @@ static int __init slc90e66_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit slc90e66_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(slc90e66_ide_init);
+module_exit(slc90e66_ide_exit);
 
 MODULE_AUTHOR("Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for SLC90E66 IDE");
diff --git a/drivers/ide/pci/tc86c001.c b/drivers/ide/pci/tc86c001.c
index 477e1979010..b1cb8a9ce5a 100644
--- a/drivers/ide/pci/tc86c001.c
+++ b/drivers/ide/pci/tc86c001.c
@@ -11,6 +11,8 @@
 #include <linux/pci.h>
 #include <linux/ide.h>
 
+#define DRV_NAME "tc86c001"
+
 static void tc86c001_set_mode(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif	= HWIF(drive);
@@ -173,16 +175,6 @@ static void __devinit init_hwif_tc86c001(ide_hwif_t *hwif)
 	hwif->rqsize	 = 0xffff;
 }
 
-static unsigned int __devinit init_chipset_tc86c001(struct pci_dev *dev,
-							const char *name)
-{
-	int err = pci_request_region(dev, 5, name);
-
-	if (err)
-		printk(KERN_ERR "%s: system control regs already in use", name);
-	return err;
-}
-
 static const struct ide_port_ops tc86c001_port_ops = {
 	.set_pio_mode		= tc86c001_set_pio_mode,
 	.set_dma_mode		= tc86c001_set_mode,
@@ -201,8 +193,7 @@ static const struct ide_dma_ops tc86c001_dma_ops = {
 };
 
 static const struct ide_port_info tc86c001_chipset __devinitdata = {
-	.name		= "TC86C001",
-	.init_chipset	= init_chipset_tc86c001,
+	.name		= DRV_NAME,
 	.init_hwif	= init_hwif_tc86c001,
 	.port_ops	= &tc86c001_port_ops,
 	.dma_ops	= &tc86c001_dma_ops,
@@ -215,7 +206,37 @@ static const struct ide_port_info tc86c001_chipset __devinitdata = {
 static int __devinit tc86c001_init_one(struct pci_dev *dev,
 				       const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &tc86c001_chipset);
+	int rc;
+
+	rc = pci_enable_device(dev);
+	if (rc)
+		goto out;
+
+	rc = pci_request_region(dev, 5, DRV_NAME);
+	if (rc) {
+		printk(KERN_ERR DRV_NAME ": system control regs already in use");
+		goto out_disable;
+	}
+
+	rc = ide_pci_init_one(dev, &tc86c001_chipset, NULL);
+	if (rc)
+		goto out_release;
+
+	goto out;
+
+out_release:
+	pci_release_region(dev, 5);
+out_disable:
+	pci_disable_device(dev);
+out:
+	return rc;
+}
+
+static void __devexit tc86c001_remove(struct pci_dev *dev)
+{
+	ide_pci_remove(dev);
+	pci_release_region(dev, 5);
+	pci_disable_device(dev);
 }
 
 static const struct pci_device_id tc86c001_pci_tbl[] = {
@@ -227,14 +248,22 @@ MODULE_DEVICE_TABLE(pci, tc86c001_pci_tbl);
 static struct pci_driver driver = {
 	.name		= "TC86C001",
 	.id_table	= tc86c001_pci_tbl,
-	.probe		= tc86c001_init_one
+	.probe		= tc86c001_init_one,
+	.remove		= tc86c001_remove,
 };
 
 static int __init tc86c001_ide_init(void)
 {
 	return ide_pci_register_driver(&driver);
 }
+
+static void __exit tc86c001_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(tc86c001_ide_init);
+module_exit(tc86c001_ide_exit);
 
 MODULE_AUTHOR("MontaVista Software, Inc. <source@mvista.com>");
 MODULE_DESCRIPTION("PCI driver module for TC86C001 IDE");
diff --git a/drivers/ide/pci/triflex.c b/drivers/ide/pci/triflex.c
index db65a558d4e..b77ec35151b 100644
--- a/drivers/ide/pci/triflex.c
+++ b/drivers/ide/pci/triflex.c
@@ -33,6 +33,8 @@
 #include <linux/ide.h>
 #include <linux/init.h>
 
+#define DRV_NAME "triflex"
+
 static void triflex_set_mode(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif = HWIF(drive);
@@ -93,7 +95,7 @@ static const struct ide_port_ops triflex_port_ops = {
 };
 
 static const struct ide_port_info triflex_device __devinitdata = {
-	.name		= "TRIFLEX",
+	.name		= DRV_NAME,
 	.enablebits	= {{0x80, 0x01, 0x01}, {0x80, 0x02, 0x02}},
 	.port_ops	= &triflex_port_ops,
 	.pio_mask	= ATA_PIO4,
@@ -104,7 +106,7 @@ static const struct ide_port_info triflex_device __devinitdata = {
 static int __devinit triflex_init_one(struct pci_dev *dev, 
 		const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &triflex_device);
+	return ide_pci_init_one(dev, &triflex_device, NULL);
 }
 
 static const struct pci_device_id triflex_pci_tbl[] = {
@@ -117,6 +119,7 @@ static struct pci_driver driver = {
 	.name		= "TRIFLEX_IDE",
 	.id_table	= triflex_pci_tbl,
 	.probe		= triflex_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init triflex_ide_init(void)
@@ -124,7 +127,13 @@ static int __init triflex_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit triflex_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(triflex_ide_init);
+module_exit(triflex_ide_exit);
 
 MODULE_AUTHOR("Torben Mathiasen");
 MODULE_DESCRIPTION("PCI driver module for Compaq Triflex IDE");
diff --git a/drivers/ide/pci/trm290.c b/drivers/ide/pci/trm290.c
index a8a3138682e..fd28b49977f 100644
--- a/drivers/ide/pci/trm290.c
+++ b/drivers/ide/pci/trm290.c
@@ -141,6 +141,8 @@
 
 #include <asm/io.h>
 
+#define DRV_NAME "trm290"
+
 static void trm290_prepare_drive (ide_drive_t *drive, unsigned int use_dma)
 {
 	ide_hwif_t *hwif = HWIF(drive);
@@ -245,10 +247,10 @@ static void __devinit init_hwif_trm290(ide_hwif_t *hwif)
 	u8 reg = 0;
 
 	if ((dev->class & 5) && cfg_base)
-		printk(KERN_INFO "TRM290: chip");
+		printk(KERN_INFO DRV_NAME " %s: chip", pci_name(dev));
 	else {
 		cfg_base = 0x3df0;
-		printk(KERN_INFO "TRM290: using default");
+		printk(KERN_INFO DRV_NAME " %s: using default", pci_name(dev));
 	}
 	printk(KERN_CONT " config base at 0x%04x\n", cfg_base);
 	hwif->config_data = cfg_base;
@@ -325,7 +327,7 @@ static struct ide_dma_ops trm290_dma_ops = {
 };
 
 static const struct ide_port_info trm290_chipset __devinitdata = {
-	.name		= "TRM290",
+	.name		= DRV_NAME,
 	.init_hwif	= init_hwif_trm290,
 	.chipset	= ide_trm290,
 	.port_ops	= &trm290_port_ops,
@@ -340,7 +342,7 @@ static const struct ide_port_info trm290_chipset __devinitdata = {
 
 static int __devinit trm290_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 {
-	return ide_setup_pci_device(dev, &trm290_chipset);
+	return ide_pci_init_one(dev, &trm290_chipset, NULL);
 }
 
 static const struct pci_device_id trm290_pci_tbl[] = {
@@ -353,6 +355,7 @@ static struct pci_driver driver = {
 	.name		= "TRM290_IDE",
 	.id_table	= trm290_pci_tbl,
 	.probe		= trm290_init_one,
+	.remove		= ide_pci_remove,
 };
 
 static int __init trm290_ide_init(void)
@@ -360,7 +363,13 @@ static int __init trm290_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit trm290_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(trm290_ide_init);
+module_exit(trm290_ide_exit);
 
 MODULE_AUTHOR("Mark Lord");
 MODULE_DESCRIPTION("PCI driver module for Tekram TRM290 IDE");
diff --git a/drivers/ide/pci/via82cxxx.c b/drivers/ide/pci/via82cxxx.c
index 09dc4803ef9..454d2bf62dc 100644
--- a/drivers/ide/pci/via82cxxx.c
+++ b/drivers/ide/pci/via82cxxx.c
@@ -35,6 +35,8 @@
 #include <asm/processor.h>
 #endif
 
+#define DRV_NAME "via82cxxx"
+
 #define VIA_IDE_ENABLE		0x40
 #define VIA_IDE_CONFIG		0x41
 #define VIA_FIFO_CONFIG		0x43
@@ -113,7 +115,8 @@ struct via82cxxx_dev
 static void via_set_speed(ide_hwif_t *hwif, u8 dn, struct ide_timing *timing)
 {
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
-	struct via82cxxx_dev *vdev = pci_get_drvdata(dev);
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct via82cxxx_dev *vdev = host->host_priv;
 	u8 t;
 
 	if (~vdev->via_config->flags & VIA_BAD_AST) {
@@ -153,7 +156,8 @@ static void via_set_drive(ide_drive_t *drive, const u8 speed)
 	ide_hwif_t *hwif = drive->hwif;
 	ide_drive_t *peer = hwif->drives + (~drive->dn & 1);
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
-	struct via82cxxx_dev *vdev = pci_get_drvdata(dev);
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct via82cxxx_dev *vdev = host->host_priv;
 	struct ide_timing t, p;
 	unsigned int T, UT;
 
@@ -258,37 +262,19 @@ static void __devinit via_cable_detect(struct via82cxxx_dev *vdev, u32 u)
 /**
  *	init_chipset_via82cxxx	-	initialization handler
  *	@dev: PCI device
- *	@name: Name of interface
  *
  *	The initialization callback. Here we determine the IDE chip type
  *	and initialize its drive independent registers.
  */
 
-static unsigned int __devinit init_chipset_via82cxxx(struct pci_dev *dev, const char *name)
+static unsigned int __devinit init_chipset_via82cxxx(struct pci_dev *dev)
 {
-	struct pci_dev *isa = NULL;
-	struct via82cxxx_dev *vdev;
-	struct via_isa_bridge *via_config;
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct via82cxxx_dev *vdev = host->host_priv;
+	struct via_isa_bridge *via_config = vdev->via_config;
 	u8 t, v;
 	u32 u;
 
-	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
-	if (!vdev) {
-		printk(KERN_ERR "VP_IDE: out of memory :(\n");
-		return -ENOMEM;
-	}
-	pci_set_drvdata(dev, vdev);
-
-	/*
-	 * Find the ISA bridge to see how good the IDE is.
-	 */
-	vdev->via_config = via_config = via_config_find(&isa);
-
-	/* We checked this earlier so if it fails here deeep badness
-	   is involved */
-
-	BUG_ON(!via_config->id);
-
 	/*
 	 * Detect cable and configure Clk66
 	 */
@@ -334,39 +320,6 @@ static unsigned int __devinit init_chipset_via82cxxx(struct pci_dev *dev, const
 
 	pci_write_config_byte(dev, VIA_FIFO_CONFIG, t);
 
-	/*
-	 * Determine system bus clock.
-	 */
-
-	via_clock = (ide_pci_clk ? ide_pci_clk : 33) * 1000;
-
-	switch (via_clock) {
-		case 33000: via_clock = 33333; break;
-		case 37000: via_clock = 37500; break;
-		case 41000: via_clock = 41666; break;
-	}
-
-	if (via_clock < 20000 || via_clock > 50000) {
-		printk(KERN_WARNING "VP_IDE: User given PCI clock speed "
-			"impossible (%d), using 33 MHz instead.\n", via_clock);
-		printk(KERN_WARNING "VP_IDE: Use ide0=ata66 if you want "
-			"to assume 80-wire cable.\n");
-		via_clock = 33333;
-	}
-
-	/*
-	 * Print the boot message.
-	 */
-
-	printk(KERN_INFO "VP_IDE: VIA %s (rev %02x) IDE %sDMA%s "
-		"controller on pci%s\n",
-		via_config->name, isa->revision,
-		via_config->udma_mask ? "U" : "MW",
-		via_dma[via_config->udma_mask ?
-			(fls(via_config->udma_mask) - 1) : 0],
-		pci_name(dev));
-
-	pci_dev_put(isa);
 	return 0;
 }
 
@@ -402,7 +355,8 @@ static int via_cable_override(struct pci_dev *pdev)
 static u8 __devinit via82cxxx_cable_detect(ide_hwif_t *hwif)
 {
 	struct pci_dev *pdev = to_pci_dev(hwif->dev);
-	struct via82cxxx_dev *vdev = pci_get_drvdata(pdev);
+	struct ide_host *host = pci_get_drvdata(pdev);
+	struct via82cxxx_dev *vdev = host->host_priv;
 
 	if (via_cable_override(pdev))
 		return ATA_CBL_PATA40_SHORT;
@@ -420,7 +374,7 @@ static const struct ide_port_ops via_port_ops = {
 };
 
 static const struct ide_port_info via82cxxx_chipset __devinitdata = {
-	.name		= "VP_IDE",
+	.name		= DRV_NAME,
 	.init_chipset	= init_chipset_via82cxxx,
 	.enablebits	= { { 0x40, 0x02, 0x02 }, { 0x40, 0x01, 0x01 } },
 	.port_ops	= &via_port_ops,
@@ -436,6 +390,8 @@ static int __devinit via_init_one(struct pci_dev *dev, const struct pci_device_i
 {
 	struct pci_dev *isa = NULL;
 	struct via_isa_bridge *via_config;
+	struct via82cxxx_dev *vdev;
+	int rc;
 	u8 idx = id->driver_data;
 	struct ide_port_info d;
 
@@ -445,12 +401,42 @@ static int __devinit via_init_one(struct pci_dev *dev, const struct pci_device_i
 	 * Find the ISA bridge and check we know what it is.
 	 */
 	via_config = via_config_find(&isa);
-	pci_dev_put(isa);
 	if (!via_config->id) {
-		printk(KERN_WARNING "VP_IDE: Unknown VIA SouthBridge, disabling DMA.\n");
+		printk(KERN_WARNING DRV_NAME " %s: unknown chipset, skipping\n",
+			pci_name(dev));
 		return -ENODEV;
 	}
 
+	/*
+	 * Print the boot message.
+	 */
+	printk(KERN_INFO DRV_NAME " %s: VIA %s (rev %02x) IDE %sDMA%s\n",
+		pci_name(dev), via_config->name, isa->revision,
+		via_config->udma_mask ? "U" : "MW",
+		via_dma[via_config->udma_mask ?
+			(fls(via_config->udma_mask) - 1) : 0]);
+
+	pci_dev_put(isa);
+
+	/*
+	 * Determine system bus clock.
+	 */
+	via_clock = (ide_pci_clk ? ide_pci_clk : 33) * 1000;
+
+	switch (via_clock) {
+	case 33000: via_clock = 33333; break;
+	case 37000: via_clock = 37500; break;
+	case 41000: via_clock = 41666; break;
+	}
+
+	if (via_clock < 20000 || via_clock > 50000) {
+		printk(KERN_WARNING DRV_NAME ": User given PCI clock speed "
+			"impossible (%d), using 33 MHz instead.\n", via_clock);
+		printk(KERN_WARNING DRV_NAME ": Use ide0=ata66 if you want "
+			"to assume 80-wire cable.\n");
+		via_clock = 33333;
+	}
+
 	if (idx == 0)
 		d.host_flags |= IDE_HFLAG_NO_AUTODMA;
 	else
@@ -466,7 +452,29 @@ static int __devinit via_init_one(struct pci_dev *dev, const struct pci_device_i
 
 	d.udma_mask = via_config->udma_mask;
 
-	return ide_setup_pci_device(dev, &d);
+	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+	if (!vdev) {
+		printk(KERN_ERR DRV_NAME " %s: out of memory :(\n",
+			pci_name(dev));
+		return -ENOMEM;
+	}
+
+	vdev->via_config = via_config;
+
+	rc = ide_pci_init_one(dev, &d, vdev);
+	if (rc)
+		kfree(vdev);
+
+	return rc;
+}
+
+static void __devexit via_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct via82cxxx_dev *vdev = host->host_priv;
+
+	ide_pci_remove(dev);
+	kfree(vdev);
 }
 
 static const struct pci_device_id via_pci_tbl[] = {
@@ -483,6 +491,7 @@ static struct pci_driver driver = {
 	.name 		= "VIA_IDE",
 	.id_table 	= via_pci_tbl,
 	.probe 		= via_init_one,
+	.remove		= via_remove,
 };
 
 static int __init via_ide_init(void)
@@ -490,7 +499,13 @@ static int __init via_ide_init(void)
 	return ide_pci_register_driver(&driver);
 }
 
+static void __exit via_ide_exit(void)
+{
+	pci_unregister_driver(&driver);
+}
+
 module_init(via_ide_init);
+module_exit(via_ide_exit);
 
 MODULE_AUTHOR("Vojtech Pavlik, Michel Aubry, Jeff Garzik, Andre Hedrick");
 MODULE_DESCRIPTION("PCI driver module for VIA IDE");
diff --git a/drivers/ide/setup-pci.c b/drivers/ide/setup-pci.c
index b15cad58dc8..a8e9e8a69a5 100644
--- a/drivers/ide/setup-pci.c
+++ b/drivers/ide/setup-pci.c
@@ -39,17 +39,18 @@ static int ide_setup_pci_baseregs(struct pci_dev *dev, const char *name)
 	if (pci_read_config_byte(dev, PCI_CLASS_PROG, &progif) ||
 			 (progif & 5) != 5) {
 		if ((progif & 0xa) != 0xa) {
-			printk(KERN_INFO "%s: device not capable of full "
-				"native PCI mode\n", name);
+			printk(KERN_INFO "%s %s: device not capable of full "
+				"native PCI mode\n", name, pci_name(dev));
 			return -EOPNOTSUPP;
 		}
-		printk("%s: placing both ports into native PCI mode\n", name);
+		printk(KERN_INFO "%s %s: placing both ports into native PCI "
+			"mode\n", name, pci_name(dev));
 		(void) pci_write_config_byte(dev, PCI_CLASS_PROG, progif|5);
 		if (pci_read_config_byte(dev, PCI_CLASS_PROG, &progif) ||
 		    (progif & 5) != 5) {
-			printk(KERN_ERR "%s: rewrite of PROGIF failed, wanted "
-				"0x%04x, got 0x%04x\n",
-				name, progif|5, progif);
+			printk(KERN_ERR "%s %s: rewrite of PROGIF failed, "
+				"wanted 0x%04x, got 0x%04x\n",
+				name, pci_name(dev), progif | 5, progif);
 			return -EOPNOTSUPP;
 		}
 	}
@@ -57,14 +58,14 @@ static int ide_setup_pci_baseregs(struct pci_dev *dev, const char *name)
 }
 
 #ifdef CONFIG_BLK_DEV_IDEDMA_PCI
-static void ide_pci_clear_simplex(unsigned long dma_base, const char *name)
+static int ide_pci_clear_simplex(unsigned long dma_base, const char *name)
 {
 	u8 dma_stat = inb(dma_base + 2);
 
 	outb(dma_stat & 0x60, dma_base + 2);
 	dma_stat = inb(dma_base + 2);
-	if (dma_stat & 0x80)
-		printk(KERN_INFO "%s: simplex device: DMA forced\n", name);
+
+	return (dma_stat & 0x80) ? 1 : 0;
 }
 
 /**
@@ -91,7 +92,8 @@ unsigned long ide_pci_dma_base(ide_hwif_t *hwif, const struct ide_port_info *d)
 		dma_base = pci_resource_start(dev, baridx);
 
 		if (dma_base == 0) {
-			printk(KERN_ERR "%s: DMA base is invalid\n", d->name);
+			printk(KERN_ERR "%s %s: DMA base is invalid\n",
+				d->name, pci_name(dev));
 			return 0;
 		}
 	}
@@ -105,13 +107,16 @@ EXPORT_SYMBOL_GPL(ide_pci_dma_base);
 
 int ide_pci_check_simplex(ide_hwif_t *hwif, const struct ide_port_info *d)
 {
+	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	u8 dma_stat;
 
 	if (d->host_flags & (IDE_HFLAG_MMIO | IDE_HFLAG_CS5520))
 		goto out;
 
 	if (d->host_flags & IDE_HFLAG_CLEAR_SIMPLEX) {
-		ide_pci_clear_simplex(hwif->dma_base, d->name);
+		if (ide_pci_clear_simplex(hwif->dma_base, d->name))
+			printk(KERN_INFO "%s %s: simplex device: DMA forced\n",
+				d->name, pci_name(dev));
 		goto out;
 	}
 
@@ -127,7 +132,8 @@ int ide_pci_check_simplex(ide_hwif_t *hwif, const struct ide_port_info *d)
 	 */
 	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
 	if ((dma_stat & 0x80) && hwif->mate && hwif->mate->dma_base) {
-		printk(KERN_INFO "%s: simplex device: DMA disabled\n", d->name);
+		printk(KERN_INFO "%s %s: simplex device: DMA disabled\n",
+			d->name, pci_name(dev));
 		return -1;
 	}
 out:
@@ -149,8 +155,8 @@ int ide_pci_set_master(struct pci_dev *dev, const char *name)
 
 		if (pci_read_config_word(dev, PCI_COMMAND, &pcicmd) ||
 		    (pcicmd & PCI_COMMAND_MASTER) == 0) {
-			printk(KERN_ERR "%s: error updating PCICMD on %s\n",
-					name, pci_name(dev));
+			printk(KERN_ERR "%s %s: error updating PCICMD\n",
+				name, pci_name(dev));
 			return -EIO;
 		}
 	}
@@ -162,9 +168,9 @@ EXPORT_SYMBOL_GPL(ide_pci_set_master);
 
 void ide_setup_pci_noise(struct pci_dev *dev, const struct ide_port_info *d)
 {
-	printk(KERN_INFO "%s: IDE controller (0x%04x:0x%04x rev 0x%02x) at "
-			 " PCI slot %s\n", d->name, dev->vendor, dev->device,
-			 dev->revision, pci_name(dev));
+	printk(KERN_INFO "%s %s: IDE controller (0x%04x:0x%04x rev 0x%02x)\n",
+		d->name, pci_name(dev),
+		dev->vendor, dev->device, dev->revision);
 }
 EXPORT_SYMBOL_GPL(ide_setup_pci_noise);
 
@@ -189,11 +195,12 @@ static int ide_pci_enable(struct pci_dev *dev, const struct ide_port_info *d)
 	if (pci_enable_device(dev)) {
 		ret = pci_enable_device_io(dev);
 		if (ret < 0) {
-			printk(KERN_WARNING "%s: (ide_setup_pci_device:) "
-				"Could not enable device.\n", d->name);
+			printk(KERN_WARNING "%s %s: couldn't enable device\n",
+				d->name, pci_name(dev));
 			goto out;
 		}
-		printk(KERN_WARNING "%s: BIOS configuration fixed.\n", d->name);
+		printk(KERN_WARNING "%s %s: BIOS configuration fixed\n",
+			d->name, pci_name(dev));
 	}
 
 	/*
@@ -203,7 +210,8 @@ static int ide_pci_enable(struct pci_dev *dev, const struct ide_port_info *d)
 	 */
 	ret = pci_set_dma_mask(dev, DMA_32BIT_MASK);
 	if (ret < 0) {
-		printk(KERN_ERR "%s: can't set dma mask\n", d->name);
+		printk(KERN_ERR "%s %s: can't set DMA mask\n",
+			d->name, pci_name(dev));
 		goto out;
 	}
 
@@ -221,7 +229,8 @@ static int ide_pci_enable(struct pci_dev *dev, const struct ide_port_info *d)
 
 	ret = pci_request_selected_regions(dev, bars, d->name);
 	if (ret < 0)
-		printk(KERN_ERR "%s: can't reserve resources\n", d->name);
+		printk(KERN_ERR "%s %s: can't reserve resources\n",
+			d->name, pci_name(dev));
 out:
 	return ret;
 }
@@ -247,15 +256,18 @@ static int ide_pci_configure(struct pci_dev *dev, const struct ide_port_info *d)
 	 */
 	if (ide_setup_pci_baseregs(dev, d->name) ||
 	    pci_write_config_word(dev, PCI_COMMAND, pcicmd | PCI_COMMAND_IO)) {
-		printk(KERN_INFO "%s: device disabled (BIOS)\n", d->name);
+		printk(KERN_INFO "%s %s: device disabled (BIOS)\n",
+			d->name, pci_name(dev));
 		return -ENODEV;
 	}
 	if (pci_read_config_word(dev, PCI_COMMAND, &pcicmd)) {
-		printk(KERN_ERR "%s: error accessing PCI regs\n", d->name);
+		printk(KERN_ERR "%s %s: error accessing PCI regs\n",
+			d->name, pci_name(dev));
 		return -EIO;
 	}
 	if (!(pcicmd & PCI_COMMAND_IO)) {
-		printk(KERN_ERR "%s: unable to enable IDE controller\n", d->name);
+		printk(KERN_ERR "%s %s: unable to enable IDE controller\n",
+			d->name, pci_name(dev));
 		return -ENXIO;
 	}
 	return 0;
@@ -311,8 +323,9 @@ static int ide_hw_configure(struct pci_dev *dev, const struct ide_port_info *d,
 	if ((d->host_flags & IDE_HFLAG_ISA_PORTS) == 0) {
 		if (ide_pci_check_iomem(dev, d, 2 * port) ||
 		    ide_pci_check_iomem(dev, d, 2 * port + 1)) {
-			printk(KERN_ERR "%s: I/O baseregs (BIOS) are reported "
-					"as MEM for port %d!\n", d->name, port);
+			printk(KERN_ERR "%s %s: I/O baseregs (BIOS) are "
+				"reported as MEM for port %d!\n",
+				d->name, pci_name(dev), port);
 			return -EINVAL;
 		}
 
@@ -325,8 +338,8 @@ static int ide_hw_configure(struct pci_dev *dev, const struct ide_port_info *d,
 	}
 
 	if (!base || !ctl) {
-		printk(KERN_ERR "%s: bad PCI BARs for port %d, skipping\n",
-				d->name, port);
+		printk(KERN_ERR "%s %s: bad PCI BARs for port %d, skipping\n",
+			d->name, pci_name(dev), port);
 		return -EINVAL;
 	}
 
@@ -393,14 +406,14 @@ int ide_hwif_setup_dma(ide_hwif_t *hwif, const struct ide_port_info *d)
  *	@dev: PCI device
  *	@d: IDE port info
  *	@noisy: verbose flag
- *	@config: returned as 1 if we configured the hardware
  *
  *	Set up the PCI and controller side of the IDE interface. This brings
  *	up the PCI side of the device, checks that the device is enabled
  *	and enables it if need be
  */
 
-static int ide_setup_pci_controller(struct pci_dev *dev, const struct ide_port_info *d, int noisy, int *config)
+static int ide_setup_pci_controller(struct pci_dev *dev,
+				    const struct ide_port_info *d, int noisy)
 {
 	int ret;
 	u16 pcicmd;
@@ -414,15 +427,16 @@ static int ide_setup_pci_controller(struct pci_dev *dev, const struct ide_port_i
 
 	ret = pci_read_config_word(dev, PCI_COMMAND, &pcicmd);
 	if (ret < 0) {
-		printk(KERN_ERR "%s: error accessing PCI regs\n", d->name);
+		printk(KERN_ERR "%s %s: error accessing PCI regs\n",
+			d->name, pci_name(dev));
 		goto out;
 	}
 	if (!(pcicmd & PCI_COMMAND_IO)) {	/* is device disabled? */
 		ret = ide_pci_configure(dev, d);
 		if (ret < 0)
 			goto out;
-		*config = 1;
-		printk(KERN_INFO "%s: device enabled (Linux)\n", d->name);
+		printk(KERN_INFO "%s %s: device enabled (Linux)\n",
+			d->name, pci_name(dev));
 	}
 
 out:
@@ -461,7 +475,8 @@ void ide_pci_setup_ports(struct pci_dev *dev, const struct ide_port_info *d,
 
 		if (e->reg && (pci_read_config_byte(dev, e->reg, &tmp) ||
 		    (tmp & e->mask) != e->val)) {
-			printk(KERN_INFO "%s: IDE port disabled\n", d->name);
+			printk(KERN_INFO "%s %s: IDE port disabled\n",
+				d->name, pci_name(dev));
 			continue;	/* port not enabled */
 		}
 
@@ -487,51 +502,35 @@ static int do_ide_setup_pci_device(struct pci_dev *dev,
 				   const struct ide_port_info *d,
 				   u8 noisy)
 {
-	int tried_config = 0;
 	int pciirq, ret;
 
-	ret = ide_setup_pci_controller(dev, d, noisy, &tried_config);
-	if (ret < 0)
-		goto out;
-
 	/*
 	 * Can we trust the reported IRQ?
 	 */
 	pciirq = dev->irq;
 
+	/*
+	 * This allows offboard ide-pci cards the enable a BIOS,
+	 * verify interrupt settings of split-mirror pci-config
+	 * space, place chipset into init-mode, and/or preserve
+	 * an interrupt if the card is not native ide support.
+	 */
+	ret = d->init_chipset ? d->init_chipset(dev) : 0;
+	if (ret < 0)
+		goto out;
+
 	/* Is it an "IDE storage" device in non-PCI mode? */
 	if ((dev->class >> 8) == PCI_CLASS_STORAGE_IDE && (dev->class & 5) != 5) {
 		if (noisy)
-			printk(KERN_INFO "%s: not 100%% native mode: "
-				"will probe irqs later\n", d->name);
-		/*
-		 * This allows offboard ide-pci cards the enable a BIOS,
-		 * verify interrupt settings of split-mirror pci-config
-		 * space, place chipset into init-mode, and/or preserve
-		 * an interrupt if the card is not native ide support.
-		 */
-		ret = d->init_chipset ? d->init_chipset(dev, d->name) : 0;
-		if (ret < 0)
-			goto out;
+			printk(KERN_INFO "%s %s: not 100%% native mode: will "
+				"probe irqs later\n", d->name, pci_name(dev));
 		pciirq = ret;
-	} else if (tried_config) {
-		if (noisy)
-			printk(KERN_INFO "%s: will probe irqs later\n", d->name);
-		pciirq = 0;
-	} else if (!pciirq) {
-		if (noisy)
-			printk(KERN_WARNING "%s: bad irq (%d): will probe later\n",
-				d->name, pciirq);
-		pciirq = 0;
-	} else {
-		if (d->init_chipset) {
-			ret = d->init_chipset(dev, d->name);
-			if (ret < 0)
-				goto out;
-		}
-		if (noisy)
-			printk(KERN_INFO "%s: 100%% native mode on irq %d\n",
-				d->name, pciirq);
+	} else if (!pciirq && noisy) {
+		printk(KERN_WARNING "%s %s: bad irq (%d): will probe later\n",
+			d->name, pci_name(dev), pciirq);
+	} else if (noisy) {
+		printk(KERN_INFO "%s %s: 100%% native mode on irq %d\n",
+			d->name, pci_name(dev), pciirq);
 	}
 
 	ret = pciirq;
@@ -539,32 +538,77 @@ out:
 	return ret;
 }
 
-int ide_setup_pci_device(struct pci_dev *dev, const struct ide_port_info *d)
+int ide_pci_init_one(struct pci_dev *dev, const struct ide_port_info *d,
+		     void *priv)
 {
+	struct ide_host *host;
 	hw_regs_t hw[4], *hws[] = { NULL, NULL, NULL, NULL };
 	int ret;
 
-	ret = do_ide_setup_pci_device(dev, d, 1);
+	ret = ide_setup_pci_controller(dev, d, 1);
+	if (ret < 0)
+		goto out;
 
-	if (ret >= 0) {
-		/* FIXME: silent failure can happen */
-		ide_pci_setup_ports(dev, d, ret, &hw[0], &hws[0]);
+	ide_pci_setup_ports(dev, d, 0, &hw[0], &hws[0]);
 
-		ret = ide_host_add(d, hws, NULL);
+	host = ide_host_alloc(d, hws);
+	if (host == NULL) {
+		ret = -ENOMEM;
+		goto out;
 	}
 
+	host->dev[0] = &dev->dev;
+
+	host->host_priv = priv;
+
+	pci_set_drvdata(dev, host);
+
+	ret = do_ide_setup_pci_device(dev, d, 1);
+	if (ret < 0)
+		goto out;
+
+	/* fixup IRQ */
+	hw[1].irq = hw[0].irq = ret;
+
+	ret = ide_host_register(host, d, hws);
+	if (ret)
+		ide_host_free(host);
+out:
 	return ret;
 }
-EXPORT_SYMBOL_GPL(ide_setup_pci_device);
+EXPORT_SYMBOL_GPL(ide_pci_init_one);
 
-int ide_setup_pci_devices(struct pci_dev *dev1, struct pci_dev *dev2,
-			  const struct ide_port_info *d)
+int ide_pci_init_two(struct pci_dev *dev1, struct pci_dev *dev2,
+		     const struct ide_port_info *d, void *priv)
 {
 	struct pci_dev *pdev[] = { dev1, dev2 };
+	struct ide_host *host;
 	int ret, i;
 	hw_regs_t hw[4], *hws[] = { NULL, NULL, NULL, NULL };
 
 	for (i = 0; i < 2; i++) {
+		ret = ide_setup_pci_controller(pdev[i], d, !i);
+		if (ret < 0)
+			goto out;
+
+		ide_pci_setup_ports(pdev[i], d, 0, &hw[i*2], &hws[i*2]);
+	}
+
+	host = ide_host_alloc(d, hws);
+	if (host == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	host->dev[0] = &dev1->dev;
+	host->dev[1] = &dev2->dev;
+
+	host->host_priv = priv;
+
+	pci_set_drvdata(pdev[0], host);
+	pci_set_drvdata(pdev[1], host);
+
+	for (i = 0; i < 2; i++) {
 		ret = do_ide_setup_pci_device(pdev[i], d, !i);
 
 		/*
@@ -574,12 +618,44 @@ int ide_setup_pci_devices(struct pci_dev *dev1, struct pci_dev *dev2,
 		if (ret < 0)
 			goto out;
 
-		/* FIXME: silent failure can happen */
-		ide_pci_setup_ports(pdev[i], d, ret, &hw[i*2], &hws[i*2]);
+		/* fixup IRQ */
+		hw[i*2 + 1].irq = hw[i*2].irq = ret;
 	}
 
-	ret = ide_host_add(d, hws, NULL);
+	ret = ide_host_register(host, d, hws);
+	if (ret)
+		ide_host_free(host);
 out:
 	return ret;
 }
-EXPORT_SYMBOL_GPL(ide_setup_pci_devices);
+EXPORT_SYMBOL_GPL(ide_pci_init_two);
+
+void ide_pci_remove(struct pci_dev *dev)
+{
+	struct ide_host *host = pci_get_drvdata(dev);
+	struct pci_dev *dev2 = host->dev[1] ? to_pci_dev(host->dev[1]) : NULL;
+	int bars;
+
+	if (host->host_flags & IDE_HFLAG_SINGLE)
+		bars = (1 << 2) - 1;
+	else
+		bars = (1 << 4) - 1;
+
+	if ((host->host_flags & IDE_HFLAG_NO_DMA) == 0) {
+		if (host->host_flags & IDE_HFLAG_CS5520)
+			bars |= (1 << 2);
+		else
+			bars |= (1 << 4);
+	}
+
+	ide_host_remove(host);
+
+	if (dev2)
+		pci_release_selected_regions(dev2, bars);
+	pci_release_selected_regions(dev, bars);
+
+	if (dev2)
+		pci_disable_device(dev2);
+	pci_disable_device(dev);
+}
+EXPORT_SYMBOL_GPL(ide_pci_remove);
diff --git a/drivers/ieee1394/iso.c b/drivers/ieee1394/iso.c
index 07ca35c98f9..1cf6487b65b 100644
--- a/drivers/ieee1394/iso.c
+++ b/drivers/ieee1394/iso.c
@@ -11,6 +11,7 @@
 
 #include <linux/pci.h>
 #include <linux/sched.h>
+#include <linux/mm.h>
 #include <linux/slab.h>
 
 #include "hosts.h"
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index ae11d5cc74d..e980ff3335d 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -168,6 +168,12 @@ struct cma_work {
 	struct rdma_cm_event	event;
 };
 
+struct cma_ndev_work {
+	struct work_struct	work;
+	struct rdma_id_private	*id;
+	struct rdma_cm_event	event;
+};
+
 union cma_ip_addr {
 	struct in6_addr ip6;
 	struct {
@@ -914,7 +920,10 @@ static int cma_ib_handler(struct ib_cm_id *cm_id, struct ib_cm_event *ib_event)
 	struct rdma_cm_event event;
 	int ret = 0;
 
-	if (cma_disable_callback(id_priv, CMA_CONNECT))
+	if ((ib_event->event != IB_CM_TIMEWAIT_EXIT &&
+		cma_disable_callback(id_priv, CMA_CONNECT)) ||
+	    (ib_event->event == IB_CM_TIMEWAIT_EXIT &&
+		cma_disable_callback(id_priv, CMA_DISCONNECT)))
 		return 0;
 
 	memset(&event, 0, sizeof event);
@@ -950,6 +959,8 @@ static int cma_ib_handler(struct ib_cm_id *cm_id, struct ib_cm_event *ib_event)
 		event.event = RDMA_CM_EVENT_DISCONNECTED;
 		break;
 	case IB_CM_TIMEWAIT_EXIT:
+		event.event = RDMA_CM_EVENT_TIMEWAIT_EXIT;
+		break;
 	case IB_CM_MRA_RECEIVED:
 		/* ignore event */
 		goto out;
@@ -1598,6 +1609,30 @@ out:
 	kfree(work);
 }
 
+static void cma_ndev_work_handler(struct work_struct *_work)
+{
+	struct cma_ndev_work *work = container_of(_work, struct cma_ndev_work, work);
+	struct rdma_id_private *id_priv = work->id;
+	int destroy = 0;
+
+	mutex_lock(&id_priv->handler_mutex);
+	if (id_priv->state == CMA_DESTROYING ||
+	    id_priv->state == CMA_DEVICE_REMOVAL)
+		goto out;
+
+	if (id_priv->id.event_handler(&id_priv->id, &work->event)) {
+		cma_exch(id_priv, CMA_DESTROYING);
+		destroy = 1;
+	}
+
+out:
+	mutex_unlock(&id_priv->handler_mutex);
+	cma_deref_id(id_priv);
+	if (destroy)
+		rdma_destroy_id(&id_priv->id);
+	kfree(work);
+}
+
 static int cma_resolve_ib_route(struct rdma_id_private *id_priv, int timeout_ms)
 {
 	struct rdma_route *route = &id_priv->id.route;
@@ -2723,6 +2758,65 @@ void rdma_leave_multicast(struct rdma_cm_id *id, struct sockaddr *addr)
 }
 EXPORT_SYMBOL(rdma_leave_multicast);
 
+static int cma_netdev_change(struct net_device *ndev, struct rdma_id_private *id_priv)
+{
+	struct rdma_dev_addr *dev_addr;
+	struct cma_ndev_work *work;
+
+	dev_addr = &id_priv->id.route.addr.dev_addr;
+
+	if ((dev_addr->src_dev == ndev) &&
+	    memcmp(dev_addr->src_dev_addr, ndev->dev_addr, ndev->addr_len)) {
+		printk(KERN_INFO "RDMA CM addr change for ndev %s used by id %p\n",
+		       ndev->name, &id_priv->id);
+		work = kzalloc(sizeof *work, GFP_KERNEL);
+		if (!work)
+			return -ENOMEM;
+
+		INIT_WORK(&work->work, cma_ndev_work_handler);
+		work->id = id_priv;
+		work->event.event = RDMA_CM_EVENT_ADDR_CHANGE;
+		atomic_inc(&id_priv->refcount);
+		queue_work(cma_wq, &work->work);
+	}
+
+	return 0;
+}
+
+static int cma_netdev_callback(struct notifier_block *self, unsigned long event,
+			       void *ctx)
+{
+	struct net_device *ndev = (struct net_device *)ctx;
+	struct cma_device *cma_dev;
+	struct rdma_id_private *id_priv;
+	int ret = NOTIFY_DONE;
+
+	if (dev_net(ndev) != &init_net)
+		return NOTIFY_DONE;
+
+	if (event != NETDEV_BONDING_FAILOVER)
+		return NOTIFY_DONE;
+
+	if (!(ndev->flags & IFF_MASTER) || !(ndev->priv_flags & IFF_BONDING))
+		return NOTIFY_DONE;
+
+	mutex_lock(&lock);
+	list_for_each_entry(cma_dev, &dev_list, list)
+		list_for_each_entry(id_priv, &cma_dev->id_list, list) {
+			ret = cma_netdev_change(ndev, id_priv);
+			if (ret)
+				goto out;
+		}
+
+out:
+	mutex_unlock(&lock);
+	return ret;
+}
+
+static struct notifier_block cma_nb = {
+	.notifier_call = cma_netdev_callback
+};
+
 static void cma_add_one(struct ib_device *device)
 {
 	struct cma_device *cma_dev;
@@ -2831,6 +2925,7 @@ static int cma_init(void)
 
 	ib_sa_register_client(&sa_client);
 	rdma_addr_register_client(&addr_client);
+	register_netdevice_notifier(&cma_nb);
 
 	ret = ib_register_client(&cma_client);
 	if (ret)
@@ -2838,6 +2933,7 @@ static int cma_init(void)
 	return 0;
 
 err:
+	unregister_netdevice_notifier(&cma_nb);
 	rdma_addr_unregister_client(&addr_client);
 	ib_sa_unregister_client(&sa_client);
 	destroy_workqueue(cma_wq);
@@ -2847,6 +2943,7 @@ err:
 static void cma_cleanup(void)
 {
 	ib_unregister_client(&cma_client);
+	unregister_netdevice_notifier(&cma_nb);
 	rdma_addr_unregister_client(&addr_client);
 	ib_sa_unregister_client(&sa_client);
 	destroy_workqueue(cma_wq);
diff --git a/drivers/infiniband/core/iwcm.c b/drivers/infiniband/core/iwcm.c
index 81c9195b512..8f9509e1ebf 100644
--- a/drivers/infiniband/core/iwcm.c
+++ b/drivers/infiniband/core/iwcm.c
@@ -942,8 +942,7 @@ static int iwcm_init_qp_init_attr(struct iwcm_id_private *cm_id_priv,
 	case IW_CM_STATE_CONN_RECV:
 	case IW_CM_STATE_ESTABLISHED:
 		*qp_attr_mask = IB_QP_STATE | IB_QP_ACCESS_FLAGS;
-		qp_attr->qp_access_flags = IB_ACCESS_LOCAL_WRITE |
-					   IB_ACCESS_REMOTE_WRITE|
+		qp_attr->qp_access_flags = IB_ACCESS_REMOTE_WRITE|
 					   IB_ACCESS_REMOTE_READ;
 		ret = 0;
 		break;
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index 1341de793e5..7863a50d56f 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -1064,7 +1064,8 @@ static void ib_sa_remove_one(struct ib_device *device)
 
 	for (i = 0; i <= sa_dev->end_port - sa_dev->start_port; ++i) {
 		ib_unregister_mad_agent(sa_dev->port[i].agent);
-		kref_put(&sa_dev->port[i].sm_ah->ref, free_sm_ah);
+		if (sa_dev->port[i].sm_ah)
+			kref_put(&sa_dev->port[i].sm_ah->ref, free_sm_ah);
 	}
 
 	kfree(sa_dev);
diff --git a/drivers/infiniband/core/ucm.c b/drivers/infiniband/core/ucm.c
index 9494005d1c9..e603736682b 100644
--- a/drivers/infiniband/core/ucm.c
+++ b/drivers/infiniband/core/ucm.c
@@ -43,7 +43,6 @@
 #include <linux/cdev.h>
 #include <linux/idr.h>
 #include <linux/mutex.h>
-#include <linux/smp_lock.h>
 
 #include <asm/uaccess.h>
 
@@ -1154,11 +1153,18 @@ static unsigned int ib_ucm_poll(struct file *filp,
 	return mask;
 }
 
+/*
+ * ib_ucm_open() does not need the BKL:
+ *
+ *  - no global state is referred to;
+ *  - there is no ioctl method to race against;
+ *  - no further module initialization is required for open to work
+ *    after the device is registered.
+ */
 static int ib_ucm_open(struct inode *inode, struct file *filp)
 {
 	struct ib_ucm_file *file;
 
-	cycle_kernel_lock();
 	file = kmalloc(sizeof(*file), GFP_KERNEL);
 	if (!file)
 		return -ENOMEM;
diff --git a/drivers/infiniband/core/ucma.c b/drivers/infiniband/core/ucma.c
index 195f97302fe..b41dd26bbfa 100644
--- a/drivers/infiniband/core/ucma.c
+++ b/drivers/infiniband/core/ucma.c
@@ -38,7 +38,6 @@
 #include <linux/in.h>
 #include <linux/in6.h>
 #include <linux/miscdevice.h>
-#include <linux/smp_lock.h>
 
 #include <rdma/rdma_user_cm.h>
 #include <rdma/ib_marshall.h>
@@ -1149,6 +1148,14 @@ static unsigned int ucma_poll(struct file *filp, struct poll_table_struct *wait)
 	return mask;
 }
 
+/*
+ * ucma_open() does not need the BKL:
+ *
+ *  - no global state is referred to;
+ *  - there is no ioctl method to race against;
+ *  - no further module initialization is required for open to work
+ *    after the device is registered.
+ */
 static int ucma_open(struct inode *inode, struct file *filp)
 {
 	struct ucma_file *file;
@@ -1157,7 +1164,6 @@ static int ucma_open(struct inode *inode, struct file *filp)
 	if (!file)
 		return -ENOMEM;
 
-	lock_kernel();
 	INIT_LIST_HEAD(&file->event_list);
 	INIT_LIST_HEAD(&file->ctx_list);
 	init_waitqueue_head(&file->poll_wait);
@@ -1165,7 +1171,6 @@ static int ucma_open(struct inode *inode, struct file *filp)
 
 	filp->private_data = file;
 	file->filp = filp;
-	unlock_kernel();
 	return 0;
 }
 
diff --git a/drivers/infiniband/hw/ehca/ehca_classes.h b/drivers/infiniband/hw/ehca/ehca_classes.h
index 1e9e99a1393..0b0618edd64 100644
--- a/drivers/infiniband/hw/ehca/ehca_classes.h
+++ b/drivers/infiniband/hw/ehca/ehca_classes.h
@@ -194,6 +194,7 @@ struct ehca_qp {
 	u32 packet_count;
 	atomic_t nr_events; /* events seen */
 	wait_queue_head_t wait_completion;
+	int mig_armed;
 };
 
 #define IS_SRQ(qp) (qp->ext_type == EQPT_SRQ)
diff --git a/drivers/infiniband/hw/ehca/ehca_hca.c b/drivers/infiniband/hw/ehca/ehca_hca.c
index bc3b37d2070..46288220cfb 100644
--- a/drivers/infiniband/hw/ehca/ehca_hca.c
+++ b/drivers/infiniband/hw/ehca/ehca_hca.c
@@ -114,7 +114,9 @@ int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props)
 	}
 
 	props->max_pkeys           = 16;
-	props->local_ca_ack_delay  = min_t(u8, rblock->local_ca_ack_delay, 255);
+	/* Some FW versions say 0 here; insert sensible value in that case */
+	props->local_ca_ack_delay  = rblock->local_ca_ack_delay ?
+		min_t(u8, rblock->local_ca_ack_delay, 255) : 12;
 	props->max_raw_ipv6_qp     = limit_uint(rblock->max_raw_ipv6_qp);
 	props->max_raw_ethy_qp     = limit_uint(rblock->max_raw_ethy_qp);
 	props->max_mcast_grp       = limit_uint(rblock->max_mcast_grp);
diff --git a/drivers/infiniband/hw/ehca/ehca_irq.c b/drivers/infiniband/hw/ehca/ehca_irq.c
index 7a64aa9b51b..cb55be04442 100644
--- a/drivers/infiniband/hw/ehca/ehca_irq.c
+++ b/drivers/infiniband/hw/ehca/ehca_irq.c
@@ -178,6 +178,10 @@ static void dispatch_qp_event(struct ehca_shca *shca, struct ehca_qp *qp,
 {
 	struct ib_event event;
 
+	/* PATH_MIG without the QP ever having been armed is false alarm */
+	if (event_type == IB_EVENT_PATH_MIG && !qp->mig_armed)
+		return;
+
 	event.device = &shca->ib_device;
 	event.event = event_type;
 
diff --git a/drivers/infiniband/hw/ehca/ehca_qp.c b/drivers/infiniband/hw/ehca/ehca_qp.c
index 3f59587338e..ea13efddf17 100644
--- a/drivers/infiniband/hw/ehca/ehca_qp.c
+++ b/drivers/infiniband/hw/ehca/ehca_qp.c
@@ -1460,6 +1460,8 @@ static int internal_modify_qp(struct ib_qp *ibqp,
 			goto modify_qp_exit2;
 		}
 		mqpcb->path_migration_state = attr->path_mig_state + 1;
+		if (attr->path_mig_state == IB_MIG_REARM)
+			my_qp->mig_armed = 1;
 		update_mask |=
 			EHCA_BMASK_SET(MQPCB_MASK_PATH_MIGRATION_STATE, 1);
 	}
diff --git a/drivers/infiniband/hw/ehca/ipz_pt_fn.c b/drivers/infiniband/hw/ehca/ipz_pt_fn.c
index 661f8db6270..c3a32846543 100644
--- a/drivers/infiniband/hw/ehca/ipz_pt_fn.c
+++ b/drivers/infiniband/hw/ehca/ipz_pt_fn.c
@@ -163,6 +163,7 @@ static int alloc_small_queue_page(struct ipz_queue *queue, struct ehca_pd *pd)
 
 out:
 	ehca_err(pd->ib_pd.device, "failed to allocate small queue page");
+	mutex_unlock(&pd->lock);
 	return 0;
 }
 
diff --git a/drivers/infiniband/hw/ipath/ipath_sdma.c b/drivers/infiniband/hw/ipath/ipath_sdma.c
index eaba03273e4..284c9bca517 100644
--- a/drivers/infiniband/hw/ipath/ipath_sdma.c
+++ b/drivers/infiniband/hw/ipath/ipath_sdma.c
@@ -698,7 +698,7 @@ retry:
 
 	addr = dma_map_single(&dd->pcidev->dev, tx->txreq.map_addr,
 			      tx->map_len, DMA_TO_DEVICE);
-	if (dma_mapping_error(addr)) {
+	if (dma_mapping_error(&dd->pcidev->dev, addr)) {
 		ret = -EIO;
 		goto unlock;
 	}
diff --git a/drivers/infiniband/hw/ipath/ipath_user_sdma.c b/drivers/infiniband/hw/ipath/ipath_user_sdma.c
index 86e016916cd..82d9a0b5ca2 100644
--- a/drivers/infiniband/hw/ipath/ipath_user_sdma.c
+++ b/drivers/infiniband/hw/ipath/ipath_user_sdma.c
@@ -206,7 +206,7 @@ static int ipath_user_sdma_coalesce(const struct ipath_devdata *dd,
 
 	dma_addr = dma_map_page(&dd->pcidev->dev, page, 0, len,
 				DMA_TO_DEVICE);
-	if (dma_mapping_error(dma_addr)) {
+	if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
 		ret = -ENOMEM;
 		goto free_unmap;
 	}
@@ -301,7 +301,7 @@ static int ipath_user_sdma_pin_pages(const struct ipath_devdata *dd,
 				     pages[j], 0, flen, DMA_TO_DEVICE);
 		unsigned long fofs = addr & ~PAGE_MASK;
 
-		if (dma_mapping_error(dma_addr)) {
+		if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
 			ret = -ENOMEM;
 			goto done;
 		}
@@ -508,7 +508,7 @@ static int ipath_user_sdma_queue_pkts(const struct ipath_devdata *dd,
 		if (page) {
 			dma_addr = dma_map_page(&dd->pcidev->dev,
 						page, 0, len, DMA_TO_DEVICE);
-			if (dma_mapping_error(dma_addr)) {
+			if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
 				ret = -ENOMEM;
 				goto free_pbc;
 			}
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index 299f20832ab..a1464574bfd 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -637,6 +638,7 @@ repoll:
 		case MLX4_OPCODE_SEND_IMM:
 			wc->wc_flags |= IB_WC_WITH_IMM;
 		case MLX4_OPCODE_SEND:
+		case MLX4_OPCODE_SEND_INVAL:
 			wc->opcode    = IB_WC_SEND;
 			break;
 		case MLX4_OPCODE_RDMA_READ:
@@ -657,6 +659,12 @@ repoll:
 		case MLX4_OPCODE_LSO:
 			wc->opcode    = IB_WC_LSO;
 			break;
+		case MLX4_OPCODE_FMR:
+			wc->opcode    = IB_WC_FAST_REG_MR;
+			break;
+		case MLX4_OPCODE_LOCAL_INVAL:
+			wc->opcode    = IB_WC_LOCAL_INV;
+			break;
 		}
 	} else {
 		wc->byte_len = be32_to_cpu(cqe->byte_cnt);
@@ -667,6 +675,11 @@ repoll:
 			wc->wc_flags	= IB_WC_WITH_IMM;
 			wc->ex.imm_data = cqe->immed_rss_invalid;
 			break;
+		case MLX4_RECV_OPCODE_SEND_INVAL:
+			wc->opcode	= IB_WC_RECV;
+			wc->wc_flags	= IB_WC_WITH_INVALIDATE;
+			wc->ex.invalidate_rkey = be32_to_cpu(cqe->immed_rss_invalid);
+			break;
 		case MLX4_RECV_OPCODE_SEND:
 			wc->opcode   = IB_WC_RECV;
 			wc->wc_flags = 0;
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index bcf50648fa1..a3c2851c054 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -104,6 +105,12 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 		props->device_cap_flags |= IB_DEVICE_UD_IP_CSUM;
 	if (dev->dev->caps.max_gso_sz)
 		props->device_cap_flags |= IB_DEVICE_UD_TSO;
+	if (dev->dev->caps.bmme_flags & MLX4_BMME_FLAG_RESERVED_LKEY)
+		props->device_cap_flags |= IB_DEVICE_LOCAL_DMA_LKEY;
+	if ((dev->dev->caps.bmme_flags & MLX4_BMME_FLAG_LOCAL_INV) &&
+	    (dev->dev->caps.bmme_flags & MLX4_BMME_FLAG_REMOTE_INV) &&
+	    (dev->dev->caps.bmme_flags & MLX4_BMME_FLAG_FAST_REG_WR))
+		props->device_cap_flags |= IB_DEVICE_MEM_MGT_EXTENSIONS;
 
 	props->vendor_id	   = be32_to_cpup((__be32 *) (out_mad->data + 36)) &
 		0xffffff;
@@ -127,6 +134,7 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 	props->max_srq		   = dev->dev->caps.num_srqs - dev->dev->caps.reserved_srqs;
 	props->max_srq_wr	   = dev->dev->caps.max_srq_wqes - 1;
 	props->max_srq_sge	   = dev->dev->caps.max_srq_sge;
+	props->max_fast_reg_page_list_len = PAGE_SIZE / sizeof (u64);
 	props->local_ca_ack_delay  = dev->dev->caps.local_ca_ack_delay;
 	props->atomic_cap	   = dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_ATOMIC ?
 		IB_ATOMIC_HCA : IB_ATOMIC_NONE;
@@ -565,6 +573,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	strlcpy(ibdev->ib_dev.name, "mlx4_%d", IB_DEVICE_NAME_MAX);
 	ibdev->ib_dev.owner		= THIS_MODULE;
 	ibdev->ib_dev.node_type		= RDMA_NODE_IB_CA;
+	ibdev->ib_dev.local_dma_lkey	= dev->caps.reserved_lkey;
 	ibdev->ib_dev.phys_port_cnt	= dev->caps.num_ports;
 	ibdev->ib_dev.num_comp_vectors	= 1;
 	ibdev->ib_dev.dma_device	= &dev->pdev->dev;
@@ -627,6 +636,9 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	ibdev->ib_dev.get_dma_mr	= mlx4_ib_get_dma_mr;
 	ibdev->ib_dev.reg_user_mr	= mlx4_ib_reg_user_mr;
 	ibdev->ib_dev.dereg_mr		= mlx4_ib_dereg_mr;
+	ibdev->ib_dev.alloc_fast_reg_mr = mlx4_ib_alloc_fast_reg_mr;
+	ibdev->ib_dev.alloc_fast_reg_page_list = mlx4_ib_alloc_fast_reg_page_list;
+	ibdev->ib_dev.free_fast_reg_page_list  = mlx4_ib_free_fast_reg_page_list;
 	ibdev->ib_dev.attach_mcast	= mlx4_ib_mcg_attach;
 	ibdev->ib_dev.detach_mcast	= mlx4_ib_mcg_detach;
 	ibdev->ib_dev.process_mad	= mlx4_ib_process_mad;
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index c4cf5b69eef..6e2b0dc21b6 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems.  All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -83,6 +84,11 @@ struct mlx4_ib_mr {
 	struct ib_umem	       *umem;
 };
 
+struct mlx4_ib_fast_reg_page_list {
+	struct ib_fast_reg_page_list	ibfrpl;
+	dma_addr_t			map;
+};
+
 struct mlx4_ib_fmr {
 	struct ib_fmr           ibfmr;
 	struct mlx4_fmr         mfmr;
@@ -199,6 +205,11 @@ static inline struct mlx4_ib_mr *to_mmr(struct ib_mr *ibmr)
 	return container_of(ibmr, struct mlx4_ib_mr, ibmr);
 }
 
+static inline struct mlx4_ib_fast_reg_page_list *to_mfrpl(struct ib_fast_reg_page_list *ibfrpl)
+{
+	return container_of(ibfrpl, struct mlx4_ib_fast_reg_page_list, ibfrpl);
+}
+
 static inline struct mlx4_ib_fmr *to_mfmr(struct ib_fmr *ibfmr)
 {
 	return container_of(ibfmr, struct mlx4_ib_fmr, ibfmr);
@@ -239,6 +250,11 @@ struct ib_mr *mlx4_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				  u64 virt_addr, int access_flags,
 				  struct ib_udata *udata);
 int mlx4_ib_dereg_mr(struct ib_mr *mr);
+struct ib_mr *mlx4_ib_alloc_fast_reg_mr(struct ib_pd *pd,
+					int max_page_list_len);
+struct ib_fast_reg_page_list *mlx4_ib_alloc_fast_reg_page_list(struct ib_device *ibdev,
+							       int page_list_len);
+void mlx4_ib_free_fast_reg_page_list(struct ib_fast_reg_page_list *page_list);
 
 int mlx4_ib_modify_cq(struct ib_cq *cq, u16 cq_count, u16 cq_period);
 int mlx4_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata);
diff --git a/drivers/infiniband/hw/mlx4/mr.c b/drivers/infiniband/hw/mlx4/mr.c
index 68e92485fc7..a4cdb465cd1 100644
--- a/drivers/infiniband/hw/mlx4/mr.c
+++ b/drivers/infiniband/hw/mlx4/mr.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -183,6 +184,76 @@ int mlx4_ib_dereg_mr(struct ib_mr *ibmr)
 	return 0;
 }
 
+struct ib_mr *mlx4_ib_alloc_fast_reg_mr(struct ib_pd *pd,
+					int max_page_list_len)
+{
+	struct mlx4_ib_dev *dev = to_mdev(pd->device);
+	struct mlx4_ib_mr *mr;
+	int err;
+
+	mr = kmalloc(sizeof *mr, GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(-ENOMEM);
+
+	err = mlx4_mr_alloc(dev->dev, to_mpd(pd)->pdn, 0, 0, 0,
+			    max_page_list_len, 0, &mr->mmr);
+	if (err)
+		goto err_free;
+
+	err = mlx4_mr_enable(dev->dev, &mr->mmr);
+	if (err)
+		goto err_mr;
+
+	return &mr->ibmr;
+
+err_mr:
+	mlx4_mr_free(dev->dev, &mr->mmr);
+
+err_free:
+	kfree(mr);
+	return ERR_PTR(err);
+}
+
+struct ib_fast_reg_page_list *mlx4_ib_alloc_fast_reg_page_list(struct ib_device *ibdev,
+							       int page_list_len)
+{
+	struct mlx4_ib_dev *dev = to_mdev(ibdev);
+	struct mlx4_ib_fast_reg_page_list *mfrpl;
+	int size = page_list_len * sizeof (u64);
+
+	if (size > PAGE_SIZE)
+		return ERR_PTR(-EINVAL);
+
+	mfrpl = kmalloc(sizeof *mfrpl, GFP_KERNEL);
+	if (!mfrpl)
+		return ERR_PTR(-ENOMEM);
+
+	mfrpl->ibfrpl.page_list = dma_alloc_coherent(&dev->dev->pdev->dev,
+						     size, &mfrpl->map,
+						     GFP_KERNEL);
+	if (!mfrpl->ibfrpl.page_list)
+		goto err_free;
+
+	WARN_ON(mfrpl->map & 0x3f);
+
+	return &mfrpl->ibfrpl;
+
+err_free:
+	kfree(mfrpl);
+	return ERR_PTR(-ENOMEM);
+}
+
+void mlx4_ib_free_fast_reg_page_list(struct ib_fast_reg_page_list *page_list)
+{
+	struct mlx4_ib_dev *dev = to_mdev(page_list->device);
+	struct mlx4_ib_fast_reg_page_list *mfrpl = to_mfrpl(page_list);
+	int size = page_list->max_page_list_len * sizeof (u64);
+
+	dma_free_coherent(&dev->dev->pdev->dev, size, page_list->page_list,
+			  mfrpl->map);
+	kfree(mfrpl);
+}
+
 struct ib_fmr *mlx4_ib_fmr_alloc(struct ib_pd *pd, int acc,
 				 struct ib_fmr_attr *fmr_attr)
 {
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 89eb6cbe592..f7bc7dd8578 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -78,6 +79,9 @@ static const __be32 mlx4_ib_opcode[] = {
 	[IB_WR_RDMA_READ]		= __constant_cpu_to_be32(MLX4_OPCODE_RDMA_READ),
 	[IB_WR_ATOMIC_CMP_AND_SWP]	= __constant_cpu_to_be32(MLX4_OPCODE_ATOMIC_CS),
 	[IB_WR_ATOMIC_FETCH_AND_ADD]	= __constant_cpu_to_be32(MLX4_OPCODE_ATOMIC_FA),
+	[IB_WR_SEND_WITH_INV]		= __constant_cpu_to_be32(MLX4_OPCODE_SEND_INVAL),
+	[IB_WR_LOCAL_INV]		= __constant_cpu_to_be32(MLX4_OPCODE_LOCAL_INVAL),
+	[IB_WR_FAST_REG_MR]		= __constant_cpu_to_be32(MLX4_OPCODE_FMR),
 };
 
 static struct mlx4_ib_sqp *to_msqp(struct mlx4_ib_qp *mqp)
@@ -976,6 +980,10 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	context->pd	    = cpu_to_be32(to_mpd(ibqp->pd)->pdn);
 	context->params1    = cpu_to_be32(MLX4_IB_ACK_REQ_FREQ << 28);
 
+	/* Set "fast registration enabled" for all kernel QPs */
+	if (!qp->ibqp.uobject)
+		context->params1 |= cpu_to_be32(1 << 11);
+
 	if (attr_mask & IB_QP_RNR_RETRY) {
 		context->params1 |= cpu_to_be32(attr->rnr_retry << 13);
 		optpar |= MLX4_QP_OPTPAR_RNR_RETRY;
@@ -1322,6 +1330,38 @@ static int mlx4_wq_overflow(struct mlx4_ib_wq *wq, int nreq, struct ib_cq *ib_cq
 	return cur + nreq >= wq->max_post;
 }
 
+static __be32 convert_access(int acc)
+{
+	return (acc & IB_ACCESS_REMOTE_ATOMIC ? cpu_to_be32(MLX4_WQE_FMR_PERM_ATOMIC)       : 0) |
+	       (acc & IB_ACCESS_REMOTE_WRITE  ? cpu_to_be32(MLX4_WQE_FMR_PERM_REMOTE_WRITE) : 0) |
+	       (acc & IB_ACCESS_REMOTE_READ   ? cpu_to_be32(MLX4_WQE_FMR_PERM_REMOTE_READ)  : 0) |
+	       (acc & IB_ACCESS_LOCAL_WRITE   ? cpu_to_be32(MLX4_WQE_FMR_PERM_LOCAL_WRITE)  : 0) |
+		cpu_to_be32(MLX4_WQE_FMR_PERM_LOCAL_READ);
+}
+
+static void set_fmr_seg(struct mlx4_wqe_fmr_seg *fseg, struct ib_send_wr *wr)
+{
+	struct mlx4_ib_fast_reg_page_list *mfrpl = to_mfrpl(wr->wr.fast_reg.page_list);
+
+	fseg->flags		= convert_access(wr->wr.fast_reg.access_flags);
+	fseg->mem_key		= cpu_to_be32(wr->wr.fast_reg.rkey);
+	fseg->buf_list		= cpu_to_be64(mfrpl->map);
+	fseg->start_addr	= cpu_to_be64(wr->wr.fast_reg.iova_start);
+	fseg->reg_len		= cpu_to_be64(wr->wr.fast_reg.length);
+	fseg->offset		= 0; /* XXX -- is this just for ZBVA? */
+	fseg->page_size		= cpu_to_be32(wr->wr.fast_reg.page_shift);
+	fseg->reserved[0]	= 0;
+	fseg->reserved[1]	= 0;
+}
+
+static void set_local_inv_seg(struct mlx4_wqe_local_inval_seg *iseg, u32 rkey)
+{
+	iseg->flags	= 0;
+	iseg->mem_key	= cpu_to_be32(rkey);
+	iseg->guest_id	= 0;
+	iseg->pa	= 0;
+}
+
 static __always_inline void set_raddr_seg(struct mlx4_wqe_raddr_seg *rseg,
 					  u64 remote_addr, u32 rkey)
 {
@@ -1395,7 +1435,7 @@ static void __set_data_seg(struct mlx4_wqe_data_seg *dseg, struct ib_sge *sg)
 	dseg->addr       = cpu_to_be64(sg->addr);
 }
 
-static int build_lso_seg(struct mlx4_lso_seg *wqe, struct ib_send_wr *wr,
+static int build_lso_seg(struct mlx4_wqe_lso_seg *wqe, struct ib_send_wr *wr,
 			 struct mlx4_ib_qp *qp, unsigned *lso_seg_len)
 {
 	unsigned halign = ALIGN(sizeof *wqe + wr->wr.ud.hlen, 16);
@@ -1423,6 +1463,21 @@ static int build_lso_seg(struct mlx4_lso_seg *wqe, struct ib_send_wr *wr,
 	return 0;
 }
 
+static __be32 send_ieth(struct ib_send_wr *wr)
+{
+	switch (wr->opcode) {
+	case IB_WR_SEND_WITH_IMM:
+	case IB_WR_RDMA_WRITE_WITH_IMM:
+		return wr->ex.imm_data;
+
+	case IB_WR_SEND_WITH_INV:
+		return cpu_to_be32(wr->ex.invalidate_rkey);
+
+	default:
+		return 0;
+	}
+}
+
 int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		      struct ib_send_wr **bad_wr)
 {
@@ -1469,11 +1524,7 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 				     MLX4_WQE_CTRL_TCP_UDP_CSUM) : 0) |
 			qp->sq_signal_bits;
 
-		if (wr->opcode == IB_WR_SEND_WITH_IMM ||
-		    wr->opcode == IB_WR_RDMA_WRITE_WITH_IMM)
-			ctrl->imm = wr->ex.imm_data;
-		else
-			ctrl->imm = 0;
+		ctrl->imm = send_ieth(wr);
 
 		wqe += sizeof *ctrl;
 		size = sizeof *ctrl / 16;
@@ -1505,6 +1556,18 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 				size += sizeof (struct mlx4_wqe_raddr_seg) / 16;
 				break;
 
+			case IB_WR_LOCAL_INV:
+				set_local_inv_seg(wqe, wr->ex.invalidate_rkey);
+				wqe  += sizeof (struct mlx4_wqe_local_inval_seg);
+				size += sizeof (struct mlx4_wqe_local_inval_seg) / 16;
+				break;
+
+			case IB_WR_FAST_REG_MR:
+				set_fmr_seg(wqe, wr);
+				wqe  += sizeof (struct mlx4_wqe_fmr_seg);
+				size += sizeof (struct mlx4_wqe_fmr_seg) / 16;
+				break;
+
 			default:
 				/* No extra segments required for sends */
 				break;
diff --git a/drivers/infiniband/hw/mlx4/srq.c b/drivers/infiniband/hw/mlx4/srq.c
index 12d6bc6f800..d42565258fb 100644
--- a/drivers/infiniband/hw/mlx4/srq.c
+++ b/drivers/infiniband/hw/mlx4/srq.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx4/user.h b/drivers/infiniband/hw/mlx4/user.h
index e2d11be4525..13beedeeef9 100644
--- a/drivers/infiniband/hw/mlx4/user.h
+++ b/drivers/infiniband/hw/mlx4/user.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mthca/mthca_dev.h b/drivers/infiniband/hw/mthca/mthca_dev.h
index ee4d073c889..252590116df 100644
--- a/drivers/infiniband/hw/mthca/mthca_dev.h
+++ b/drivers/infiniband/hw/mthca/mthca_dev.h
@@ -202,6 +202,7 @@ struct mthca_pd_table {
 
 struct mthca_buddy {
 	unsigned long **bits;
+	int	       *num_free;
 	int             max_order;
 	spinlock_t      lock;
 };
diff --git a/drivers/infiniband/hw/mthca/mthca_eq.c b/drivers/infiniband/hw/mthca/mthca_eq.c
index 4e36aa7cb3d..cc6858f0b65 100644
--- a/drivers/infiniband/hw/mthca/mthca_eq.c
+++ b/drivers/infiniband/hw/mthca/mthca_eq.c
@@ -780,7 +780,7 @@ int mthca_map_eq_icm(struct mthca_dev *dev, u64 icm_virt)
 		return -ENOMEM;
 	dev->eq_table.icm_dma  = pci_map_page(dev->pdev, dev->eq_table.icm_page, 0,
 					      PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-	if (pci_dma_mapping_error(dev->eq_table.icm_dma)) {
+	if (pci_dma_mapping_error(dev->pdev, dev->eq_table.icm_dma)) {
 		__free_page(dev->eq_table.icm_page);
 		return -ENOMEM;
 	}
diff --git a/drivers/infiniband/hw/mthca/mthca_mr.c b/drivers/infiniband/hw/mthca/mthca_mr.c
index 8489b1e81c0..882e6b73591 100644
--- a/drivers/infiniband/hw/mthca/mthca_mr.c
+++ b/drivers/infiniband/hw/mthca/mthca_mr.c
@@ -89,23 +89,26 @@ static u32 mthca_buddy_alloc(struct mthca_buddy *buddy, int order)
 
 	spin_lock(&buddy->lock);
 
-	for (o = order; o <= buddy->max_order; ++o) {
-		m = 1 << (buddy->max_order - o);
-		seg = find_first_bit(buddy->bits[o], m);
-		if (seg < m)
-			goto found;
-	}
+	for (o = order; o <= buddy->max_order; ++o)
+		if (buddy->num_free[o]) {
+			m = 1 << (buddy->max_order - o);
+			seg = find_first_bit(buddy->bits[o], m);
+			if (seg < m)
+				goto found;
+		}
 
 	spin_unlock(&buddy->lock);
 	return -1;
 
  found:
 	clear_bit(seg, buddy->bits[o]);
+	--buddy->num_free[o];
 
 	while (o > order) {
 		--o;
 		seg <<= 1;
 		set_bit(seg ^ 1, buddy->bits[o]);
+		++buddy->num_free[o];
 	}
 
 	spin_unlock(&buddy->lock);
@@ -123,11 +126,13 @@ static void mthca_buddy_free(struct mthca_buddy *buddy, u32 seg, int order)
 
 	while (test_bit(seg ^ 1, buddy->bits[order])) {
 		clear_bit(seg ^ 1, buddy->bits[order]);
+		--buddy->num_free[order];
 		seg >>= 1;
 		++order;
 	}
 
 	set_bit(seg, buddy->bits[order]);
+	++buddy->num_free[order];
 
 	spin_unlock(&buddy->lock);
 }
@@ -141,7 +146,9 @@ static int mthca_buddy_init(struct mthca_buddy *buddy, int max_order)
 
 	buddy->bits = kzalloc((buddy->max_order + 1) * sizeof (long *),
 			      GFP_KERNEL);
-	if (!buddy->bits)
+	buddy->num_free = kzalloc((buddy->max_order + 1) * sizeof (int *),
+				  GFP_KERNEL);
+	if (!buddy->bits || !buddy->num_free)
 		goto err_out;
 
 	for (i = 0; i <= buddy->max_order; ++i) {
@@ -154,6 +161,7 @@ static int mthca_buddy_init(struct mthca_buddy *buddy, int max_order)
 	}
 
 	set_bit(0, buddy->bits[buddy->max_order]);
+	buddy->num_free[buddy->max_order] = 1;
 
 	return 0;
 
@@ -161,9 +169,10 @@ err_out_free:
 	for (i = 0; i <= buddy->max_order; ++i)
 		kfree(buddy->bits[i]);
 
+err_out:
 	kfree(buddy->bits);
+	kfree(buddy->num_free);
 
-err_out:
 	return -ENOMEM;
 }
 
@@ -175,6 +184,7 @@ static void mthca_buddy_cleanup(struct mthca_buddy *buddy)
 		kfree(buddy->bits[i]);
 
 	kfree(buddy->bits);
+	kfree(buddy->num_free);
 }
 
 static u32 mthca_alloc_mtt_range(struct mthca_dev *dev, int order,
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index d2884e77809..b0cab64e5e3 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -276,6 +276,7 @@ static void nes_cqp_rem_ref_callback(struct nes_device *nesdev, struct nes_cqp_r
 	}
 	nes_free_resource(nesadapter, nesadapter->allocated_qps, nesqp->hwqp.qp_id);
 
+	nesadapter->qp_table[nesqp->hwqp.qp_id-NES_FIRST_QPN] = NULL;
 	kfree(nesqp->allocated_buffer);
 
 }
@@ -289,7 +290,6 @@ void nes_rem_ref(struct ib_qp *ibqp)
 	struct nes_qp *nesqp;
 	struct nes_vnic *nesvnic = to_nesvnic(ibqp->device);
 	struct nes_device *nesdev = nesvnic->nesdev;
-	struct nes_adapter *nesadapter = nesdev->nesadapter;
 	struct nes_hw_cqp_wqe *cqp_wqe;
 	struct nes_cqp_request *cqp_request;
 	u32 opcode;
@@ -303,8 +303,6 @@ void nes_rem_ref(struct ib_qp *ibqp)
 	}
 
 	if (atomic_dec_and_test(&nesqp->refcount)) {
-		nesadapter->qp_table[nesqp->hwqp.qp_id-NES_FIRST_QPN] = NULL;
-
 		/* Destroy the QP */
 		cqp_request = nes_get_cqp_request(nesdev);
 		if (cqp_request == NULL) {
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 6aa531d5276..9f0b964b2c9 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -74,36 +74,59 @@ atomic_t cm_nodes_destroyed;
 atomic_t cm_accel_dropped_pkts;
 atomic_t cm_resets_recvd;
 
-static inline int mini_cm_accelerated(struct nes_cm_core *, struct nes_cm_node *);
+static inline int mini_cm_accelerated(struct nes_cm_core *,
+	struct nes_cm_node *);
 static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *,
-		struct nes_vnic *, struct nes_cm_info *);
-static int add_ref_cm_node(struct nes_cm_node *);
-static int rem_ref_cm_node(struct nes_cm_core *, struct nes_cm_node *);
+	struct nes_vnic *, struct nes_cm_info *);
 static int mini_cm_del_listen(struct nes_cm_core *, struct nes_cm_listener *);
-static struct sk_buff *form_cm_frame(struct sk_buff *, struct nes_cm_node *,
-				     void *, u32, void *, u32, u8);
-static struct sk_buff *get_free_pkt(struct nes_cm_node *cm_node);
-
 static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *,
-					   struct nes_vnic *,
-					   struct ietf_mpa_frame *,
-					   struct nes_cm_info *);
+	struct nes_vnic *, u16, void *, struct nes_cm_info *);
+static int mini_cm_close(struct nes_cm_core *, struct nes_cm_node *);
 static int mini_cm_accept(struct nes_cm_core *, struct ietf_mpa_frame *,
-			  struct nes_cm_node *);
+	struct nes_cm_node *);
 static int mini_cm_reject(struct nes_cm_core *, struct ietf_mpa_frame *,
-			  struct nes_cm_node *);
-static int mini_cm_close(struct nes_cm_core *, struct nes_cm_node *);
-static int mini_cm_recv_pkt(struct nes_cm_core *, struct nes_vnic *,
-			    struct sk_buff *);
+	struct nes_cm_node *);
+static void mini_cm_recv_pkt(struct nes_cm_core *, struct nes_vnic *,
+	struct sk_buff *);
 static int mini_cm_dealloc_core(struct nes_cm_core *);
 static int mini_cm_get(struct nes_cm_core *);
 static int mini_cm_set(struct nes_cm_core *, u32, u32);
+
+static struct sk_buff *form_cm_frame(struct sk_buff *, struct nes_cm_node *,
+	void *, u32, void *, u32, u8);
+static struct sk_buff *get_free_pkt(struct nes_cm_node *cm_node);
+static int add_ref_cm_node(struct nes_cm_node *);
+static int rem_ref_cm_node(struct nes_cm_core *, struct nes_cm_node *);
+
 static int nes_cm_disconn_true(struct nes_qp *);
 static int nes_cm_post_event(struct nes_cm_event *event);
 static int nes_disconnect(struct nes_qp *nesqp, int abrupt);
 static void nes_disconnect_worker(struct work_struct *work);
-static int send_ack(struct nes_cm_node *cm_node);
+
+static int send_mpa_request(struct nes_cm_node *, struct sk_buff *);
+static int send_syn(struct nes_cm_node *, u32, struct sk_buff *);
+static int send_reset(struct nes_cm_node *, struct sk_buff *);
+static int send_ack(struct nes_cm_node *cm_node, struct sk_buff *skb);
 static int send_fin(struct nes_cm_node *cm_node, struct sk_buff *skb);
+static void process_packet(struct nes_cm_node *, struct sk_buff *,
+	struct nes_cm_core *);
+
+static void active_open_err(struct nes_cm_node *, struct sk_buff *, int);
+static void passive_open_err(struct nes_cm_node *, struct sk_buff *, int);
+static void cleanup_retrans_entry(struct nes_cm_node *);
+static void handle_rcv_mpa(struct nes_cm_node *, struct sk_buff *,
+	enum nes_cm_event_type);
+static void free_retrans_entry(struct nes_cm_node *cm_node);
+static int handle_tcp_options(struct nes_cm_node *cm_node, struct tcphdr *tcph,
+	struct sk_buff *skb, int optionsize, int passive);
+
+/* CM event handler functions */
+static void cm_event_connected(struct nes_cm_event *);
+static void cm_event_connect_error(struct nes_cm_event *);
+static void cm_event_reset(struct nes_cm_event *);
+static void cm_event_mpa_req(struct nes_cm_event *);
+
+static void print_core(struct nes_cm_core *core);
 
 /* External CM API Interface */
 /* instance of function pointers for client API */
@@ -158,11 +181,11 @@ static struct nes_cm_event *create_event(struct nes_cm_node *cm_node,
 	event->cm_info.loc_port = cm_node->loc_port;
 	event->cm_info.cm_id = cm_node->cm_id;
 
-	nes_debug(NES_DBG_CM, "Created event=%p, type=%u, dst_addr=%08x[%x],"
-			" src_addr=%08x[%x]\n",
-			event, type,
-			event->cm_info.loc_addr, event->cm_info.loc_port,
-			event->cm_info.rem_addr, event->cm_info.rem_port);
+	nes_debug(NES_DBG_CM, "cm_node=%p Created event=%p, type=%u, "
+		"dst_addr=%08x[%x], src_addr=%08x[%x]\n",
+		cm_node, event, type, event->cm_info.loc_addr,
+		event->cm_info.loc_port, event->cm_info.rem_addr,
+		event->cm_info.rem_port);
 
 	nes_cm_post_event(event);
 	return event;
@@ -172,14 +195,11 @@ static struct nes_cm_event *create_event(struct nes_cm_node *cm_node,
 /**
  * send_mpa_request
  */
-static int send_mpa_request(struct nes_cm_node *cm_node)
+static int send_mpa_request(struct nes_cm_node *cm_node, struct sk_buff *skb)
 {
-	struct sk_buff *skb;
 	int ret;
-
-	skb = get_free_pkt(cm_node);
 	if (!skb) {
-		nes_debug(NES_DBG_CM, "Failed to get a Free pkt\n");
+		nes_debug(NES_DBG_CM, "skb set to NULL\n");
 		return -1;
 	}
 
@@ -188,9 +208,8 @@ static int send_mpa_request(struct nes_cm_node *cm_node)
 			cm_node->mpa_frame_size, SET_ACK);
 
 	ret = schedule_nes_timer(cm_node, skb, NES_TIMER_TYPE_SEND, 1, 0);
-	if (ret < 0) {
+	if (ret < 0)
 		return ret;
-	}
 
 	return 0;
 }
@@ -229,46 +248,12 @@ static int parse_mpa(struct nes_cm_node *cm_node, u8 *buffer, u32 len)
 
 
 /**
- * handle_exception_pkt - process an exception packet.
- * We have been in a TSA state, and we have now received SW
- * TCP/IP traffic should be a FIN request or IP pkt with options
- */
-static int handle_exception_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb)
-{
-	int ret = 0;
-	struct tcphdr *tcph = tcp_hdr(skb);
-
-	/* first check to see if this a FIN pkt */
-	if (tcph->fin) {
-		/* we need to ACK the FIN request */
-		send_ack(cm_node);
-
-		/* check which side we are (client/server) and set next state accordingly */
-		if (cm_node->tcp_cntxt.client)
-			cm_node->state = NES_CM_STATE_CLOSING;
-		else {
-			/* we are the server side */
-			cm_node->state = NES_CM_STATE_CLOSE_WAIT;
-			/* since this is a self contained CM we don't wait for */
-			/* an APP to close us, just send final FIN immediately */
-			ret = send_fin(cm_node, NULL);
-			cm_node->state = NES_CM_STATE_LAST_ACK;
-		}
-	} else {
-		ret = -EINVAL;
-	}
-
-	return ret;
-}
-
-
-/**
  * form_cm_frame - get a free packet and build empty frame Use
  * node info to build.
  */
-static struct sk_buff *form_cm_frame(struct sk_buff *skb, struct nes_cm_node *cm_node,
-				     void *options, u32 optionsize, void *data,
-				     u32 datasize, u8 flags)
+static struct sk_buff *form_cm_frame(struct sk_buff *skb,
+	struct nes_cm_node *cm_node, void *options, u32 optionsize,
+	void *data, u32 datasize, u8 flags)
 {
 	struct tcphdr *tcph;
 	struct iphdr *iph;
@@ -332,10 +317,12 @@ static struct sk_buff *form_cm_frame(struct sk_buff *skb, struct nes_cm_node *cm
 		cm_node->tcp_cntxt.loc_seq_num++;
 		tcph->syn = 1;
 	} else
-		cm_node->tcp_cntxt.loc_seq_num += datasize;	/* data (no headers) */
+		cm_node->tcp_cntxt.loc_seq_num += datasize;
 
-	if (flags & SET_FIN)
+	if (flags & SET_FIN) {
+		cm_node->tcp_cntxt.loc_seq_num++;
 		tcph->fin = 1;
+	}
 
 	if (flags & SET_RST)
 		tcph->rst = 1;
@@ -389,7 +376,7 @@ int schedule_nes_timer(struct nes_cm_node *cm_node, struct sk_buff *skb,
 		int close_when_complete)
 {
 	unsigned long  flags;
-	struct nes_cm_core *cm_core;
+	struct nes_cm_core *cm_core = cm_node->cm_core;
 	struct nes_timer_entry *new_send;
 	int ret = 0;
 	u32 was_timer_set;
@@ -411,7 +398,7 @@ int schedule_nes_timer(struct nes_cm_node *cm_node, struct sk_buff *skb,
 	new_send->close_when_complete = close_when_complete;
 
 	if (type == NES_TIMER_TYPE_CLOSE) {
-		new_send->timetosend += (HZ/2);	/* TODO: decide on the correct value here */
+		new_send->timetosend += (HZ/10);
 		spin_lock_irqsave(&cm_node->recv_list_lock, flags);
 		list_add_tail(&new_send->list, &cm_node->recv_list);
 		spin_unlock_irqrestore(&cm_node->recv_list_lock, flags);
@@ -420,36 +407,28 @@ int schedule_nes_timer(struct nes_cm_node *cm_node, struct sk_buff *skb,
 	if (type == NES_TIMER_TYPE_SEND) {
 		new_send->seq_num = ntohl(tcp_hdr(skb)->seq);
 		atomic_inc(&new_send->skb->users);
+		spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
+		cm_node->send_entry = new_send;
+		add_ref_cm_node(cm_node);
+		spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
+		new_send->timetosend = jiffies + NES_RETRY_TIMEOUT;
 
 		ret = nes_nic_cm_xmit(new_send->skb, cm_node->netdev);
 		if (ret != NETDEV_TX_OK) {
-			nes_debug(NES_DBG_CM, "Error sending packet %p (jiffies = %lu)\n",
-					new_send, jiffies);
+			nes_debug(NES_DBG_CM, "Error sending packet %p "
+				"(jiffies = %lu)\n", new_send, jiffies);
 			atomic_dec(&new_send->skb->users);
 			new_send->timetosend = jiffies;
 		} else {
 			cm_packets_sent++;
 			if (!send_retrans) {
+				cleanup_retrans_entry(cm_node);
 				if (close_when_complete)
-					rem_ref_cm_node(cm_node->cm_core, cm_node);
-				dev_kfree_skb_any(new_send->skb);
-				kfree(new_send);
+					rem_ref_cm_node(cm_core, cm_node);
 				return ret;
 			}
-			new_send->timetosend = jiffies + NES_RETRY_TIMEOUT;
 		}
-		spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-		list_add_tail(&new_send->list, &cm_node->retrans_list);
-		spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-	}
-	if (type == NES_TIMER_TYPE_RECV) {
-		new_send->seq_num = ntohl(tcp_hdr(skb)->seq);
-		new_send->timetosend = jiffies;
-		spin_lock_irqsave(&cm_node->recv_list_lock, flags);
-		list_add_tail(&new_send->list, &cm_node->recv_list);
-		spin_unlock_irqrestore(&cm_node->recv_list_lock, flags);
 	}
-	cm_core = cm_node->cm_core;
 
 	was_timer_set = timer_pending(&cm_core->tcp_timer);
 
@@ -476,23 +455,27 @@ static void nes_cm_timer_tick(unsigned long pass)
 	struct list_head *list_node, *list_node_temp;
 	struct nes_cm_core *cm_core = g_cm_core;
 	struct nes_qp *nesqp;
-	struct sk_buff *skb;
 	u32 settimer = 0;
 	int ret = NETDEV_TX_OK;
-	int    node_done;
+	enum nes_cm_node_state last_state;
 
 	spin_lock_irqsave(&cm_core->ht_lock, flags);
 
-	list_for_each_safe(list_node, list_core_temp, &cm_core->connected_nodes) {
+	list_for_each_safe(list_node, list_core_temp,
+		&cm_core->connected_nodes) {
 		cm_node = container_of(list_node, struct nes_cm_node, list);
 		add_ref_cm_node(cm_node);
 		spin_unlock_irqrestore(&cm_core->ht_lock, flags);
 		spin_lock_irqsave(&cm_node->recv_list_lock, flags);
-		list_for_each_safe(list_core, list_node_temp, &cm_node->recv_list) {
-			recv_entry = container_of(list_core, struct nes_timer_entry, list);
-			if ((time_after(recv_entry->timetosend, jiffies)) &&
-					(recv_entry->type == NES_TIMER_TYPE_CLOSE)) {
-				if (nexttimeout > recv_entry->timetosend || !settimer) {
+		list_for_each_safe(list_core, list_node_temp,
+			&cm_node->recv_list) {
+			recv_entry = container_of(list_core,
+				struct nes_timer_entry, list);
+			if (!recv_entry)
+				break;
+			if (time_after(recv_entry->timetosend, jiffies)) {
+				if (nexttimeout > recv_entry->timetosend ||
+					!settimer) {
 					nexttimeout = recv_entry->timetosend;
 					settimer = 1;
 				}
@@ -501,157 +484,143 @@ static void nes_cm_timer_tick(unsigned long pass)
 			list_del(&recv_entry->list);
 			cm_id = cm_node->cm_id;
 			spin_unlock_irqrestore(&cm_node->recv_list_lock, flags);
-			if (recv_entry->type == NES_TIMER_TYPE_CLOSE) {
-				nesqp = (struct nes_qp *)recv_entry->skb;
-				spin_lock_irqsave(&nesqp->lock, qplockflags);
-				if (nesqp->cm_id) {
-					nes_debug(NES_DBG_CM, "QP%u: cm_id = %p, refcount = %d: "
-							"****** HIT A NES_TIMER_TYPE_CLOSE"
-							" with something to do!!! ******\n",
-							nesqp->hwqp.qp_id, cm_id,
-							atomic_read(&nesqp->refcount));
-					nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-					nesqp->last_aeq = NES_AEQE_AEID_RESET_SENT;
-					nesqp->ibqp_state = IB_QPS_ERR;
-					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_cm_disconn(nesqp);
-				} else {
-					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_debug(NES_DBG_CM, "QP%u: cm_id = %p, refcount = %d:"
-							" ****** HIT A NES_TIMER_TYPE_CLOSE"
-							" with nothing to do!!! ******\n",
-							nesqp->hwqp.qp_id, cm_id,
-							atomic_read(&nesqp->refcount));
-					nes_rem_ref(&nesqp->ibqp);
-				}
-				if (cm_id)
-					cm_id->rem_ref(cm_id);
+			nesqp = (struct nes_qp *)recv_entry->skb;
+			spin_lock_irqsave(&nesqp->lock, qplockflags);
+			if (nesqp->cm_id) {
+				nes_debug(NES_DBG_CM, "QP%u: cm_id = %p, "
+					"refcount = %d: HIT A "
+					"NES_TIMER_TYPE_CLOSE with something "
+					"to do!!!\n", nesqp->hwqp.qp_id, cm_id,
+					atomic_read(&nesqp->refcount));
+				nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
+				nesqp->last_aeq = NES_AEQE_AEID_RESET_SENT;
+				nesqp->ibqp_state = IB_QPS_ERR;
+				spin_unlock_irqrestore(&nesqp->lock,
+					qplockflags);
+				nes_cm_disconn(nesqp);
+			} else {
+				spin_unlock_irqrestore(&nesqp->lock,
+					qplockflags);
+				nes_debug(NES_DBG_CM, "QP%u: cm_id = %p, "
+					"refcount = %d: HIT A "
+					"NES_TIMER_TYPE_CLOSE with nothing "
+					"to do!!!\n", nesqp->hwqp.qp_id, cm_id,
+					atomic_read(&nesqp->refcount));
 			}
+			if (cm_id)
+				cm_id->rem_ref(cm_id);
+
 			kfree(recv_entry);
 			spin_lock_irqsave(&cm_node->recv_list_lock, flags);
 		}
 		spin_unlock_irqrestore(&cm_node->recv_list_lock, flags);
 
 		spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-		node_done = 0;
-		list_for_each_safe(list_core, list_node_temp, &cm_node->retrans_list) {
-			if (node_done) {
-				break;
-			}
-			send_entry = container_of(list_core, struct nes_timer_entry, list);
+		do {
+			send_entry = cm_node->send_entry;
+			if (!send_entry)
+				continue;
 			if (time_after(send_entry->timetosend, jiffies)) {
 				if (cm_node->state != NES_CM_STATE_TSA) {
-					if ((nexttimeout > send_entry->timetosend) || !settimer) {
-						nexttimeout = send_entry->timetosend;
+					if ((nexttimeout >
+						send_entry->timetosend) ||
+						!settimer) {
+						nexttimeout =
+							send_entry->timetosend;
 						settimer = 1;
+						continue;
 					}
-					node_done = 1;
-					continue;
 				} else {
-					list_del(&send_entry->list);
-					skb = send_entry->skb;
-					spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-					dev_kfree_skb_any(skb);
-					kfree(send_entry);
-					spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
+					free_retrans_entry(cm_node);
 					continue;
 				}
 			}
-			if (send_entry->type == NES_TIMER_NODE_CLEANUP) {
-				list_del(&send_entry->list);
-				spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-				kfree(send_entry);
-				spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-				continue;
-			}
-			if ((send_entry->seq_num < cm_node->tcp_cntxt.rem_ack_num) ||
-					(cm_node->state == NES_CM_STATE_TSA) ||
-					(cm_node->state == NES_CM_STATE_CLOSED)) {
-				skb = send_entry->skb;
-				list_del(&send_entry->list);
-				spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-				kfree(send_entry);
-				dev_kfree_skb_any(skb);
-				spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
+
+			if ((cm_node->state == NES_CM_STATE_TSA) ||
+				(cm_node->state == NES_CM_STATE_CLOSED)) {
+				free_retrans_entry(cm_node);
 				continue;
 			}
 
-			if (!send_entry->retranscount || !send_entry->retrycount) {
+			if (!send_entry->retranscount ||
+				!send_entry->retrycount) {
 				cm_packets_dropped++;
-				skb = send_entry->skb;
-				list_del(&send_entry->list);
-				spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-				dev_kfree_skb_any(skb);
-				kfree(send_entry);
-				if (cm_node->state == NES_CM_STATE_SYN_RCVD) {
-					/* this node never even generated an indication up to the cm */
+				last_state = cm_node->state;
+				cm_node->state = NES_CM_STATE_CLOSED;
+				free_retrans_entry(cm_node);
+				spin_unlock_irqrestore(
+					&cm_node->retrans_list_lock, flags);
+				if (last_state == NES_CM_STATE_SYN_RCVD)
 					rem_ref_cm_node(cm_core, cm_node);
-				} else {
-					cm_node->state = NES_CM_STATE_CLOSED;
-					create_event(cm_node, NES_CM_EVENT_ABORTED);
-				}
-				spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
+				else
+					create_event(cm_node,
+						NES_CM_EVENT_ABORTED);
+				spin_lock_irqsave(&cm_node->retrans_list_lock,
+					flags);
 				continue;
 			}
-			/* this seems like the correct place, but leave send entry unprotected */
-			/* spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags); */
 			atomic_inc(&send_entry->skb->users);
 			cm_packets_retrans++;
-			nes_debug(NES_DBG_CM, "Retransmitting send_entry %p for node %p,"
-					" jiffies = %lu, time to send =  %lu, retranscount = %u, "
-					"send_entry->seq_num = 0x%08X, cm_node->tcp_cntxt.rem_ack_num = 0x%08X\n",
-					send_entry, cm_node, jiffies, send_entry->timetosend, send_entry->retranscount,
-					send_entry->seq_num, cm_node->tcp_cntxt.rem_ack_num);
-
-			spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
+			nes_debug(NES_DBG_CM, "Retransmitting send_entry %p "
+				"for node %p, jiffies = %lu, time to send = "
+				"%lu, retranscount = %u, send_entry->seq_num = "
+				"0x%08X, cm_node->tcp_cntxt.rem_ack_num = "
+				"0x%08X\n", send_entry, cm_node, jiffies,
+				send_entry->timetosend,
+				send_entry->retranscount,
+				send_entry->seq_num,
+				cm_node->tcp_cntxt.rem_ack_num);
+
+			spin_unlock_irqrestore(&cm_node->retrans_list_lock,
+				flags);
 			ret = nes_nic_cm_xmit(send_entry->skb, cm_node->netdev);
+			spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
 			if (ret != NETDEV_TX_OK) {
+				nes_debug(NES_DBG_CM, "rexmit failed for "
+					"node=%p\n", cm_node);
 				cm_packets_bounced++;
 				atomic_dec(&send_entry->skb->users);
 				send_entry->retrycount--;
 				nexttimeout = jiffies + NES_SHORT_TIME;
 				settimer = 1;
-				node_done = 1;
-				spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
 				continue;
 			} else {
 				cm_packets_sent++;
 			}
-			spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-			list_del(&send_entry->list);
-			nes_debug(NES_DBG_CM, "Packet Sent: retrans count = %u, retry count = %u.\n",
-					send_entry->retranscount, send_entry->retrycount);
+			nes_debug(NES_DBG_CM, "Packet Sent: retrans count = "
+				"%u, retry count = %u.\n",
+				send_entry->retranscount,
+				send_entry->retrycount);
 			if (send_entry->send_retrans) {
 				send_entry->retranscount--;
-				send_entry->timetosend = jiffies + NES_RETRY_TIMEOUT;
-				if (nexttimeout > send_entry->timetosend || !settimer) {
+				send_entry->timetosend = jiffies +
+					NES_RETRY_TIMEOUT;
+				if (nexttimeout > send_entry->timetosend ||
+					!settimer) {
 					nexttimeout = send_entry->timetosend;
 					settimer = 1;
 				}
-				list_add(&send_entry->list, &cm_node->retrans_list);
-				continue;
 			} else {
 				int close_when_complete;
-				skb = send_entry->skb;
-				close_when_complete = send_entry->close_when_complete;
-				spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-				if (close_when_complete) {
-					BUG_ON(atomic_read(&cm_node->ref_count) == 1);
-					rem_ref_cm_node(cm_core, cm_node);
-				}
-				dev_kfree_skb_any(skb);
-				kfree(send_entry);
-				spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-				continue;
+				close_when_complete =
+					send_entry->close_when_complete;
+				nes_debug(NES_DBG_CM, "cm_node=%p state=%d\n",
+					cm_node, cm_node->state);
+				free_retrans_entry(cm_node);
+				if (close_when_complete)
+					rem_ref_cm_node(cm_node->cm_core,
+						cm_node);
 			}
-		}
-		spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-
-		rem_ref_cm_node(cm_core, cm_node);
+		} while (0);
 
+		spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
+		rem_ref_cm_node(cm_node->cm_core, cm_node);
 		spin_lock_irqsave(&cm_core->ht_lock, flags);
-		if (ret != NETDEV_TX_OK)
+		if (ret != NETDEV_TX_OK) {
+			nes_debug(NES_DBG_CM, "rexmit failed for cm_node=%p\n",
+				cm_node);
 			break;
+		}
 	}
 	spin_unlock_irqrestore(&cm_core->ht_lock, flags);
 
@@ -667,14 +636,14 @@ static void nes_cm_timer_tick(unsigned long pass)
 /**
  * send_syn
  */
-static int send_syn(struct nes_cm_node *cm_node, u32 sendack)
+static int send_syn(struct nes_cm_node *cm_node, u32 sendack,
+	struct sk_buff *skb)
 {
 	int ret;
 	int flags = SET_SYN;
-	struct sk_buff *skb;
 	char optionsbuffer[sizeof(struct option_mss) +
-			sizeof(struct option_windowscale) +
-			sizeof(struct option_base) + 1];
+		sizeof(struct option_windowscale) + sizeof(struct option_base) +
+		TCP_OPTIONS_PADDING];
 
 	int optionssize = 0;
 	/* Sending MSS option */
@@ -695,8 +664,7 @@ static int send_syn(struct nes_cm_node *cm_node, u32 sendack)
 	options->as_windowscale.shiftcount = cm_node->tcp_cntxt.rcv_wscale;
 	optionssize += sizeof(struct option_windowscale);
 
-	if (sendack && !(NES_DRV_OPT_SUPRESS_OPTION_BC & nes_drv_opt)
-			) {
+	if (sendack && !(NES_DRV_OPT_SUPRESS_OPTION_BC & nes_drv_opt)) {
 		options = (union all_known_options *)&optionsbuffer[optionssize];
 		options->as_base.optionnum = OPTION_NUMBER_WRITE0;
 		options->as_base.length = sizeof(struct option_base);
@@ -714,7 +682,8 @@ static int send_syn(struct nes_cm_node *cm_node, u32 sendack)
 	options->as_end = OPTION_NUMBER_END;
 	optionssize += 1;
 
-	skb = get_free_pkt(cm_node);
+	if (!skb)
+		skb = get_free_pkt(cm_node);
 	if (!skb) {
 		nes_debug(NES_DBG_CM, "Failed to get a Free pkt\n");
 		return -1;
@@ -733,18 +702,18 @@ static int send_syn(struct nes_cm_node *cm_node, u32 sendack)
 /**
  * send_reset
  */
-static int send_reset(struct nes_cm_node *cm_node)
+static int send_reset(struct nes_cm_node *cm_node, struct sk_buff *skb)
 {
 	int ret;
-	struct sk_buff *skb = get_free_pkt(cm_node);
 	int flags = SET_RST | SET_ACK;
 
+	if (!skb)
+		skb = get_free_pkt(cm_node);
 	if (!skb) {
 		nes_debug(NES_DBG_CM, "Failed to get a Free pkt\n");
 		return -1;
 	}
 
-	add_ref_cm_node(cm_node);
 	form_cm_frame(skb, cm_node, NULL, 0, NULL, 0, flags);
 	ret = schedule_nes_timer(cm_node, skb, NES_TIMER_TYPE_SEND, 0, 1);
 
@@ -755,10 +724,12 @@ static int send_reset(struct nes_cm_node *cm_node)
 /**
  * send_ack
  */
-static int send_ack(struct nes_cm_node *cm_node)
+static int send_ack(struct nes_cm_node *cm_node, struct sk_buff *skb)
 {
 	int ret;
-	struct sk_buff *skb = get_free_pkt(cm_node);
+
+	if (!skb)
+		skb = get_free_pkt(cm_node);
 
 	if (!skb) {
 		nes_debug(NES_DBG_CM, "Failed to get a Free pkt\n");
@@ -922,7 +893,8 @@ static int add_hte_node(struct nes_cm_core *cm_core, struct nes_cm_node *cm_node
 	if (!cm_node || !cm_core)
 		return -EINVAL;
 
-	nes_debug(NES_DBG_CM, "Adding Node to Active Connection HT\n");
+	nes_debug(NES_DBG_CM, "Adding Node %p to Active Connection HT\n",
+		cm_node);
 
 	/* first, make an index into our hash table */
 	hashkey = make_hashkey(cm_node->loc_port, cm_node->loc_addr,
@@ -946,10 +918,35 @@ static int add_hte_node(struct nes_cm_core *cm_core, struct nes_cm_node *cm_node
  * mini_cm_dec_refcnt_listen
  */
 static int mini_cm_dec_refcnt_listen(struct nes_cm_core *cm_core,
-		struct nes_cm_listener *listener, int free_hanging_nodes)
+	struct nes_cm_listener *listener, int free_hanging_nodes)
 {
 	int ret = 1;
 	unsigned long flags;
+	struct list_head *list_pos = NULL;
+	struct list_head *list_temp = NULL;
+	struct nes_cm_node *cm_node = NULL;
+
+	nes_debug(NES_DBG_CM, "attempting listener= %p free_nodes= %d, "
+		"refcnt=%d\n", listener, free_hanging_nodes,
+		atomic_read(&listener->ref_count));
+	/* free non-accelerated child nodes for this listener */
+	if (free_hanging_nodes) {
+		spin_lock_irqsave(&cm_core->ht_lock, flags);
+		list_for_each_safe(list_pos, list_temp,
+			&g_cm_core->connected_nodes) {
+			cm_node = container_of(list_pos, struct nes_cm_node,
+				list);
+			if ((cm_node->listener == listener) &&
+				(!cm_node->accelerated)) {
+				cleanup_retrans_entry(cm_node);
+				spin_unlock_irqrestore(&cm_core->ht_lock,
+					flags);
+				send_reset(cm_node, NULL);
+				spin_lock_irqsave(&cm_core->ht_lock, flags);
+			}
+		}
+		spin_unlock_irqrestore(&cm_core->ht_lock, flags);
+	}
 	spin_lock_irqsave(&cm_core->listen_list_lock, flags);
 	if (!atomic_dec_return(&listener->ref_count)) {
 		list_del(&listener->list);
@@ -1067,18 +1064,18 @@ static struct nes_cm_node *make_cm_node(struct nes_cm_core *cm_core,
 	cm_node->loc_port = cm_info->loc_port;
 	cm_node->rem_port = cm_info->rem_port;
 	cm_node->send_write0 = send_first;
-	nes_debug(NES_DBG_CM, "Make node addresses : loc = " NIPQUAD_FMT ":%x, rem = " NIPQUAD_FMT ":%x\n",
-		  HIPQUAD(cm_node->loc_addr), cm_node->loc_port,
-		  HIPQUAD(cm_node->rem_addr), cm_node->rem_port);
+	nes_debug(NES_DBG_CM, "Make node addresses : loc = " NIPQUAD_FMT
+			":%x, rem = " NIPQUAD_FMT ":%x\n",
+			HIPQUAD(cm_node->loc_addr), cm_node->loc_port,
+			HIPQUAD(cm_node->rem_addr), cm_node->rem_port);
 	cm_node->listener = listener;
 	cm_node->netdev = nesvnic->netdev;
 	cm_node->cm_id = cm_info->cm_id;
 	memcpy(cm_node->loc_mac, nesvnic->netdev->dev_addr, ETH_ALEN);
 
-	nes_debug(NES_DBG_CM, "listener=%p, cm_id=%p\n",
-			cm_node->listener, cm_node->cm_id);
+	nes_debug(NES_DBG_CM, "listener=%p, cm_id=%p\n", cm_node->listener,
+			cm_node->cm_id);
 
-	INIT_LIST_HEAD(&cm_node->retrans_list);
 	spin_lock_init(&cm_node->retrans_list_lock);
 	INIT_LIST_HEAD(&cm_node->recv_list);
 	spin_lock_init(&cm_node->recv_list_lock);
@@ -1142,10 +1139,9 @@ static int add_ref_cm_node(struct nes_cm_node *cm_node)
  * rem_ref_cm_node - destroy an instance of a cm node
  */
 static int rem_ref_cm_node(struct nes_cm_core *cm_core,
-		struct nes_cm_node *cm_node)
+	struct nes_cm_node *cm_node)
 {
 	unsigned long flags, qplockflags;
-	struct nes_timer_entry *send_entry;
 	struct nes_timer_entry *recv_entry;
 	struct iw_cm_id *cm_id;
 	struct list_head *list_core, *list_node_temp;
@@ -1169,48 +1165,33 @@ static int rem_ref_cm_node(struct nes_cm_core *cm_core,
 		atomic_dec(&cm_node->listener->pend_accepts_cnt);
 		BUG_ON(atomic_read(&cm_node->listener->pend_accepts_cnt) < 0);
 	}
-
-	spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-	list_for_each_safe(list_core, list_node_temp, &cm_node->retrans_list) {
-		send_entry = container_of(list_core, struct nes_timer_entry, list);
-		list_del(&send_entry->list);
-		spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-		dev_kfree_skb_any(send_entry->skb);
-		kfree(send_entry);
-		spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
-		continue;
-	}
-	spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
-
+	BUG_ON(cm_node->send_entry);
 	spin_lock_irqsave(&cm_node->recv_list_lock, flags);
 	list_for_each_safe(list_core, list_node_temp, &cm_node->recv_list) {
-		recv_entry = container_of(list_core, struct nes_timer_entry, list);
+		recv_entry = container_of(list_core, struct nes_timer_entry,
+				list);
 		list_del(&recv_entry->list);
 		cm_id = cm_node->cm_id;
 		spin_unlock_irqrestore(&cm_node->recv_list_lock, flags);
-		if (recv_entry->type == NES_TIMER_TYPE_CLOSE) {
-			nesqp = (struct nes_qp *)recv_entry->skb;
-			spin_lock_irqsave(&nesqp->lock, qplockflags);
-			if (nesqp->cm_id) {
-				nes_debug(NES_DBG_CM, "QP%u: cm_id = %p: ****** HIT A NES_TIMER_TYPE_CLOSE"
-						" with something to do!!! ******\n",
-						nesqp->hwqp.qp_id, cm_id);
-				nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-				nesqp->last_aeq = NES_AEQE_AEID_RESET_SENT;
-				nesqp->ibqp_state = IB_QPS_ERR;
-				spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-				nes_cm_disconn(nesqp);
-			} else {
-				spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-				nes_debug(NES_DBG_CM, "QP%u: cm_id = %p: ****** HIT A NES_TIMER_TYPE_CLOSE"
-						" with nothing to do!!! ******\n",
-						nesqp->hwqp.qp_id, cm_id);
-				nes_rem_ref(&nesqp->ibqp);
-			}
-			cm_id->rem_ref(cm_id);
-		} else if (recv_entry->type == NES_TIMER_TYPE_RECV) {
-			dev_kfree_skb_any(recv_entry->skb);
+		nesqp = (struct nes_qp *)recv_entry->skb;
+		spin_lock_irqsave(&nesqp->lock, qplockflags);
+		if (nesqp->cm_id) {
+			nes_debug(NES_DBG_CM, "QP%u: cm_id = %p: HIT A "
+				"NES_TIMER_TYPE_CLOSE with something to do!\n",
+				nesqp->hwqp.qp_id, cm_id);
+			nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
+			nesqp->last_aeq = NES_AEQE_AEID_RESET_SENT;
+			nesqp->ibqp_state = IB_QPS_ERR;
+			spin_unlock_irqrestore(&nesqp->lock, qplockflags);
+			nes_cm_disconn(nesqp);
+		} else {
+			spin_unlock_irqrestore(&nesqp->lock, qplockflags);
+			nes_debug(NES_DBG_CM, "QP%u: cm_id = %p: HIT A "
+				"NES_TIMER_TYPE_CLOSE with nothing to do!\n",
+				nesqp->hwqp.qp_id, cm_id);
 		}
+		cm_id->rem_ref(cm_id);
+
 		kfree(recv_entry);
 		spin_lock_irqsave(&cm_node->recv_list_lock, flags);
 	}
@@ -1221,23 +1202,31 @@ static int rem_ref_cm_node(struct nes_cm_core *cm_core,
 	} else {
 		if (cm_node->apbvt_set && cm_node->nesvnic) {
 			nes_manage_apbvt(cm_node->nesvnic, cm_node->loc_port,
-					PCI_FUNC(cm_node->nesvnic->nesdev->pcidev->devfn),
-					NES_MANAGE_APBVT_DEL);
+				PCI_FUNC(
+				cm_node->nesvnic->nesdev->pcidev->devfn),
+				NES_MANAGE_APBVT_DEL);
 		}
 	}
 
-	kfree(cm_node);
 	atomic_dec(&cm_core->node_cnt);
 	atomic_inc(&cm_nodes_destroyed);
+	nesqp = cm_node->nesqp;
+	if (nesqp) {
+		nesqp->cm_node = NULL;
+		nes_rem_ref(&nesqp->ibqp);
+		cm_node->nesqp = NULL;
+	}
 
+	cm_node->freed = 1;
+	kfree(cm_node);
 	return 0;
 }
 
-
 /**
  * process_options
  */
-static int process_options(struct nes_cm_node *cm_node, u8 *optionsloc, u32 optionsize, u32 syn_packet)
+static int process_options(struct nes_cm_node *cm_node, u8 *optionsloc,
+	u32 optionsize, u32 syn_packet)
 {
 	u32 tmp;
 	u32 offset = 0;
@@ -1247,35 +1236,37 @@ static int process_options(struct nes_cm_node *cm_node, u8 *optionsloc, u32 opti
 	while (offset < optionsize) {
 		all_options = (union all_known_options *)(optionsloc + offset);
 		switch (all_options->as_base.optionnum) {
-			case OPTION_NUMBER_END:
-				offset = optionsize;
-				break;
-			case OPTION_NUMBER_NONE:
-				offset += 1;
-				continue;
-			case OPTION_NUMBER_MSS:
-				nes_debug(NES_DBG_CM, "%s: MSS Length: %d Offset: %d Size: %d\n",
-						__func__,
-						all_options->as_mss.length, offset, optionsize);
-				got_mss_option = 1;
-				if (all_options->as_mss.length != 4) {
-					return 1;
-				} else {
-					tmp = ntohs(all_options->as_mss.mss);
-					if (tmp > 0 && tmp < cm_node->tcp_cntxt.mss)
-						cm_node->tcp_cntxt.mss = tmp;
-				}
-				break;
-			case OPTION_NUMBER_WINDOW_SCALE:
-				cm_node->tcp_cntxt.snd_wscale = all_options->as_windowscale.shiftcount;
-				break;
-			case OPTION_NUMBER_WRITE0:
-				cm_node->send_write0 = 1;
-				break;
-			default:
-				nes_debug(NES_DBG_CM, "TCP Option not understood: %x\n",
-						all_options->as_base.optionnum);
-				break;
+		case OPTION_NUMBER_END:
+			offset = optionsize;
+			break;
+		case OPTION_NUMBER_NONE:
+			offset += 1;
+			continue;
+		case OPTION_NUMBER_MSS:
+			nes_debug(NES_DBG_CM, "%s: MSS Length: %d Offset: %d "
+				"Size: %d\n", __func__,
+				all_options->as_mss.length, offset, optionsize);
+			got_mss_option = 1;
+			if (all_options->as_mss.length != 4) {
+				return 1;
+			} else {
+				tmp = ntohs(all_options->as_mss.mss);
+				if (tmp > 0 && tmp <
+					cm_node->tcp_cntxt.mss)
+					cm_node->tcp_cntxt.mss = tmp;
+			}
+			break;
+		case OPTION_NUMBER_WINDOW_SCALE:
+			cm_node->tcp_cntxt.snd_wscale =
+				all_options->as_windowscale.shiftcount;
+			break;
+		case OPTION_NUMBER_WRITE0:
+			cm_node->send_write0 = 1;
+			break;
+		default:
+			nes_debug(NES_DBG_CM, "TCP Option not understood: %x\n",
+				all_options->as_base.optionnum);
+			break;
 		}
 		offset += all_options->as_base.length;
 	}
@@ -1284,300 +1275,491 @@ static int process_options(struct nes_cm_node *cm_node, u8 *optionsloc, u32 opti
 	return 0;
 }
 
+static void drop_packet(struct sk_buff *skb)
+{
+	atomic_inc(&cm_accel_dropped_pkts);
+	dev_kfree_skb_any(skb);
+}
 
-/**
- * process_packet
- */
-static int process_packet(struct nes_cm_node *cm_node, struct sk_buff *skb,
-			  struct nes_cm_core *cm_core)
+static void handle_fin_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct tcphdr *tcph)
 {
-	int optionsize;
-	int datasize;
-	int ret = 0;
-	struct tcphdr *tcph = tcp_hdr(skb);
-	u32 inc_sequence;
-	if (cm_node->state == NES_CM_STATE_SYN_SENT && tcph->syn) {
-		inc_sequence = ntohl(tcph->seq);
-		cm_node->tcp_cntxt.rcv_nxt = inc_sequence;
+	atomic_inc(&cm_resets_recvd);
+	nes_debug(NES_DBG_CM, "Received FIN, cm_node = %p, state = %u. "
+		"refcnt=%d\n", cm_node, cm_node->state,
+		atomic_read(&cm_node->ref_count));
+	cm_node->tcp_cntxt.rcv_nxt++;
+	cleanup_retrans_entry(cm_node);
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_RCVD:
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_MPAREQ_SENT:
+		cm_node->state = NES_CM_STATE_LAST_ACK;
+		send_fin(cm_node, skb);
+		break;
+	case NES_CM_STATE_FIN_WAIT1:
+		cm_node->state = NES_CM_STATE_CLOSING;
+		send_ack(cm_node, skb);
+		break;
+	case NES_CM_STATE_FIN_WAIT2:
+		cm_node->state = NES_CM_STATE_TIME_WAIT;
+		send_ack(cm_node, skb);
+		cm_node->state = NES_CM_STATE_CLOSED;
+		break;
+	case NES_CM_STATE_TSA:
+	default:
+		nes_debug(NES_DBG_CM, "Error Rcvd FIN for node-%p state = %d\n",
+			cm_node, cm_node->state);
+		drop_packet(skb);
+		break;
 	}
+}
 
-	if ((!tcph) || (cm_node->state == NES_CM_STATE_TSA)) {
-		BUG_ON(!tcph);
-		atomic_inc(&cm_accel_dropped_pkts);
-		return -1;
-	}
 
-	if (tcph->rst) {
-		atomic_inc(&cm_resets_recvd);
-		nes_debug(NES_DBG_CM, "Received Reset, cm_node = %p, state = %u. refcnt=%d\n",
-				cm_node, cm_node->state, atomic_read(&cm_node->ref_count));
-		switch (cm_node->state) {
-			case NES_CM_STATE_LISTENING:
-				rem_ref_cm_node(cm_core, cm_node);
-				break;
-			case NES_CM_STATE_TSA:
-			case NES_CM_STATE_CLOSED:
-				break;
-			case NES_CM_STATE_SYN_RCVD:
-					nes_debug(NES_DBG_CM, "Received a reset for local 0x%08X:%04X,"
-							" remote 0x%08X:%04X, node state = %u\n",
-							cm_node->loc_addr, cm_node->loc_port,
-							cm_node->rem_addr, cm_node->rem_port,
-							cm_node->state);
-				rem_ref_cm_node(cm_core, cm_node);
-				break;
-			case NES_CM_STATE_ONE_SIDE_ESTABLISHED:
-			case NES_CM_STATE_ESTABLISHED:
-			case NES_CM_STATE_MPAREQ_SENT:
-			default:
-					nes_debug(NES_DBG_CM, "Received a reset for local 0x%08X:%04X,"
-							" remote 0x%08X:%04X, node state = %u refcnt=%d\n",
-							cm_node->loc_addr, cm_node->loc_port,
-							cm_node->rem_addr, cm_node->rem_port,
-							cm_node->state, atomic_read(&cm_node->ref_count));
-				/* create event */
-				cm_node->state = NES_CM_STATE_CLOSED;
+static void handle_rst_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct tcphdr *tcph)
+{
 
-				create_event(cm_node, NES_CM_EVENT_ABORTED);
-				break;
+	int	reset = 0;	/* whether to send reset in case of err.. */
+	atomic_inc(&cm_resets_recvd);
+	nes_debug(NES_DBG_CM, "Received Reset, cm_node = %p, state = %u."
+			" refcnt=%d\n", cm_node, cm_node->state,
+			atomic_read(&cm_node->ref_count));
+	cleanup_retrans_entry(cm_node);
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_MPAREQ_SENT:
+		nes_debug(NES_DBG_CM, "%s[%u] create abort for cm_node=%p "
+			"listener=%p state=%d\n", __func__, __LINE__, cm_node,
+			cm_node->listener, cm_node->state);
+		active_open_err(cm_node, skb, reset);
+		break;
+	/* For PASSIVE open states, remove the cm_node event */
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_SYN_RCVD:
+	case NES_CM_STATE_LISTENING:
+		nes_debug(NES_DBG_CM, "Bad state %s[%u]\n", __func__, __LINE__);
+		passive_open_err(cm_node, skb, reset);
+		break;
+	case NES_CM_STATE_TSA:
+	default:
+		break;
+	}
+}
 
+static void handle_rcv_mpa(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	enum nes_cm_event_type type)
+{
+
+	int	ret;
+	int datasize = skb->len;
+	u8 *dataloc = skb->data;
+	ret = parse_mpa(cm_node, dataloc, datasize);
+	if (ret < 0) {
+		nes_debug(NES_DBG_CM, "didn't like MPA Request\n");
+		if (type == NES_CM_EVENT_CONNECTED) {
+			nes_debug(NES_DBG_CM, "%s[%u] create abort for "
+				"cm_node=%p listener=%p state=%d\n", __func__,
+				__LINE__, cm_node, cm_node->listener,
+				cm_node->state);
+			active_open_err(cm_node, skb, 1);
+		} else {
+			passive_open_err(cm_node, skb, 1);
 		}
-		return -1;
+	} else {
+		cleanup_retrans_entry(cm_node);
+		dev_kfree_skb_any(skb);
+		if (type == NES_CM_EVENT_CONNECTED)
+			cm_node->state = NES_CM_STATE_TSA;
+		create_event(cm_node, type);
+
+	}
+	return ;
+}
+
+static void indicate_pkt_err(struct nes_cm_node *cm_node, struct sk_buff *skb)
+{
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_MPAREQ_SENT:
+		nes_debug(NES_DBG_CM, "%s[%u] create abort for cm_node=%p "
+			"listener=%p state=%d\n", __func__, __LINE__, cm_node,
+			cm_node->listener, cm_node->state);
+		active_open_err(cm_node, skb, 1);
+		break;
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_SYN_RCVD:
+		passive_open_err(cm_node, skb, 1);
+		break;
+	case NES_CM_STATE_TSA:
+	default:
+		drop_packet(skb);
 	}
+}
+
+static int check_syn(struct nes_cm_node *cm_node, struct tcphdr *tcph,
+	struct sk_buff *skb)
+{
+	int err;
+
+	err = ((ntohl(tcph->ack_seq) == cm_node->tcp_cntxt.loc_seq_num))? 0 : 1;
+	if (err)
+		active_open_err(cm_node, skb, 1);
+
+	return err;
+}
+
+static int check_seq(struct nes_cm_node *cm_node, struct tcphdr *tcph,
+	struct sk_buff *skb)
+{
+	int err = 0;
+	u32 seq;
+	u32 ack_seq;
+	u32 loc_seq_num = cm_node->tcp_cntxt.loc_seq_num;
+	u32 rcv_nxt = cm_node->tcp_cntxt.rcv_nxt;
+	u32 rcv_wnd;
+	seq = ntohl(tcph->seq);
+	ack_seq = ntohl(tcph->ack_seq);
+	rcv_wnd = cm_node->tcp_cntxt.rcv_wnd;
+	if (ack_seq != loc_seq_num)
+		err = 1;
+	else if ((seq + rcv_wnd) < rcv_nxt)
+		err = 1;
+	if (err) {
+		nes_debug(NES_DBG_CM, "%s[%u] create abort for cm_node=%p "
+			"listener=%p state=%d\n", __func__, __LINE__, cm_node,
+			cm_node->listener, cm_node->state);
+		indicate_pkt_err(cm_node, skb);
+		nes_debug(NES_DBG_CM, "seq ERROR cm_node =%p seq=0x%08X "
+			"rcv_nxt=0x%08X rcv_wnd=0x%x\n", cm_node, seq, rcv_nxt,
+			rcv_wnd);
+	}
+	return err;
+}
+
+/*
+ * handle_syn_pkt() is for Passive node. The syn packet is received when a node
+ * is created with a listener or it may comein as rexmitted packet which in
+ * that case will be just dropped.
+ */
+
+static void handle_syn_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct tcphdr *tcph)
+{
+	int ret;
+	u32 inc_sequence;
+	int optionsize;
 
 	optionsize = (tcph->doff << 2) - sizeof(struct tcphdr);
+	skb_pull(skb, tcph->doff << 2);
+	inc_sequence = ntohl(tcph->seq);
 
-	skb_pull(skb, ip_hdr(skb)->ihl << 2);
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_MPAREQ_SENT:
+		/* Rcvd syn on active open connection*/
+		active_open_err(cm_node, skb, 1);
+		break;
+	case NES_CM_STATE_LISTENING:
+		/* Passive OPEN */
+		cm_node->accept_pend = 1;
+		atomic_inc(&cm_node->listener->pend_accepts_cnt);
+		if (atomic_read(&cm_node->listener->pend_accepts_cnt) >
+				cm_node->listener->backlog) {
+			nes_debug(NES_DBG_CM, "drop syn due to backlog "
+				"pressure \n");
+			cm_backlog_drops++;
+			passive_open_err(cm_node, skb, 0);
+			break;
+		}
+		ret = handle_tcp_options(cm_node, tcph, skb, optionsize,
+			1);
+		if (ret) {
+			passive_open_err(cm_node, skb, 0);
+			/* drop pkt */
+			break;
+		}
+		cm_node->tcp_cntxt.rcv_nxt = inc_sequence + 1;
+		BUG_ON(cm_node->send_entry);
+		cm_node->state = NES_CM_STATE_SYN_RCVD;
+		send_syn(cm_node, 1, skb);
+		break;
+	case NES_CM_STATE_TSA:
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_FIN_WAIT1:
+	case NES_CM_STATE_FIN_WAIT2:
+	case NES_CM_STATE_MPAREQ_RCVD:
+	case NES_CM_STATE_LAST_ACK:
+	case NES_CM_STATE_CLOSING:
+	case NES_CM_STATE_UNKNOWN:
+	case NES_CM_STATE_CLOSED:
+	default:
+		drop_packet(skb);
+		break;
+	}
+}
+
+static void handle_synack_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct tcphdr *tcph)
+{
+
+	int ret;
+	u32 inc_sequence;
+	int optionsize;
+
+	optionsize = (tcph->doff << 2) - sizeof(struct tcphdr);
 	skb_pull(skb, tcph->doff << 2);
+	inc_sequence = ntohl(tcph->seq);
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_SENT:
+		/* active open */
+		if (check_syn(cm_node, tcph, skb))
+			return;
+		cm_node->tcp_cntxt.rem_ack_num = ntohl(tcph->ack_seq);
+		/* setup options */
+		ret = handle_tcp_options(cm_node, tcph, skb, optionsize, 0);
+		if (ret) {
+			nes_debug(NES_DBG_CM, "cm_node=%p tcp_options failed\n",
+				cm_node);
+			break;
+		}
+		cleanup_retrans_entry(cm_node);
+		cm_node->tcp_cntxt.rcv_nxt = inc_sequence + 1;
+		send_mpa_request(cm_node, skb);
+		cm_node->state = NES_CM_STATE_MPAREQ_SENT;
+		break;
+	case NES_CM_STATE_MPAREQ_RCVD:
+		/* passive open, so should not be here */
+		passive_open_err(cm_node, skb, 1);
+		break;
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_FIN_WAIT1:
+	case NES_CM_STATE_FIN_WAIT2:
+	case NES_CM_STATE_LAST_ACK:
+	case NES_CM_STATE_TSA:
+	case NES_CM_STATE_CLOSING:
+	case NES_CM_STATE_UNKNOWN:
+	case NES_CM_STATE_CLOSED:
+	case NES_CM_STATE_MPAREQ_SENT:
+	default:
+		drop_packet(skb);
+		break;
+	}
+}
 
-	datasize = skb->len;
+static void handle_ack_pkt(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct tcphdr *tcph)
+{
+	int datasize = 0;
+	u32 inc_sequence;
+	u32 rem_seq_ack;
+	u32 rem_seq;
+	if (check_seq(cm_node, tcph, skb))
+		return;
+
+	skb_pull(skb, tcph->doff << 2);
 	inc_sequence = ntohl(tcph->seq);
-	nes_debug(NES_DBG_CM, "datasize = %u, sequence = 0x%08X, ack_seq = 0x%08X,"
-			" rcv_nxt = 0x%08X Flags: %s %s.\n",
-			datasize, inc_sequence, ntohl(tcph->ack_seq),
-			cm_node->tcp_cntxt.rcv_nxt, (tcph->syn ? "SYN":""),
-			(tcph->ack ? "ACK":""));
-
-	if (!tcph->syn && (inc_sequence != cm_node->tcp_cntxt.rcv_nxt)
-		) {
-		nes_debug(NES_DBG_CM, "dropping packet, datasize = %u, sequence = 0x%08X,"
-				" ack_seq = 0x%08X, rcv_nxt = 0x%08X Flags: %s.\n",
-				datasize, inc_sequence, ntohl(tcph->ack_seq),
-				cm_node->tcp_cntxt.rcv_nxt, (tcph->ack ? "ACK":""));
-		if (cm_node->state == NES_CM_STATE_LISTENING) {
-			rem_ref_cm_node(cm_core, cm_node);
+	rem_seq = ntohl(tcph->seq);
+	rem_seq_ack =  ntohl(tcph->ack_seq);
+	datasize = skb->len;
+
+	switch (cm_node->state) {
+	case NES_CM_STATE_SYN_RCVD:
+		/* Passive OPEN */
+		cm_node->tcp_cntxt.rem_ack_num = ntohl(tcph->ack_seq);
+		cm_node->state = NES_CM_STATE_ESTABLISHED;
+		if (datasize) {
+			cm_node->tcp_cntxt.rcv_nxt = inc_sequence + datasize;
+			cm_node->state = NES_CM_STATE_MPAREQ_RCVD;
+			handle_rcv_mpa(cm_node, skb, NES_CM_EVENT_MPA_REQ);
+		 } else { /* rcvd ACK only */
+			dev_kfree_skb_any(skb);
+			cleanup_retrans_entry(cm_node);
+		 }
+		break;
+	case NES_CM_STATE_ESTABLISHED:
+		/* Passive OPEN */
+		/* We expect mpa frame to be received only */
+		if (datasize) {
+			cm_node->tcp_cntxt.rcv_nxt = inc_sequence + datasize;
+			cm_node->state = NES_CM_STATE_MPAREQ_RCVD;
+			handle_rcv_mpa(cm_node, skb,
+				NES_CM_EVENT_MPA_REQ);
+		} else
+			drop_packet(skb);
+		break;
+	case NES_CM_STATE_MPAREQ_SENT:
+		cm_node->tcp_cntxt.rem_ack_num = ntohl(tcph->ack_seq);
+		if (datasize) {
+			cm_node->tcp_cntxt.rcv_nxt = inc_sequence + datasize;
+			handle_rcv_mpa(cm_node, skb, NES_CM_EVENT_CONNECTED);
+		} else { /* Could be just an ack pkt.. */
+			cleanup_retrans_entry(cm_node);
+			dev_kfree_skb_any(skb);
 		}
-		return -1;
+		break;
+	case NES_CM_STATE_FIN_WAIT1:
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_FIN_WAIT2:
+	case NES_CM_STATE_TSA:
+	case NES_CM_STATE_CLOSED:
+	case NES_CM_STATE_MPAREQ_RCVD:
+	case NES_CM_STATE_LAST_ACK:
+	case NES_CM_STATE_CLOSING:
+	case NES_CM_STATE_UNKNOWN:
+	default:
+		drop_packet(skb);
+		break;
 	}
+}
 
-		cm_node->tcp_cntxt.rcv_nxt = inc_sequence + datasize;
 
 
+static int handle_tcp_options(struct nes_cm_node *cm_node, struct tcphdr *tcph,
+	struct sk_buff *skb, int optionsize, int passive)
+{
+	u8 *optionsloc = (u8 *)&tcph[1];
 	if (optionsize) {
-		u8 *optionsloc = (u8 *)&tcph[1];
-		if (process_options(cm_node, optionsloc, optionsize, (u32)tcph->syn)) {
-			nes_debug(NES_DBG_CM, "%s: Node %p, Sending RESET\n", __func__, cm_node);
-			send_reset(cm_node);
-			if (cm_node->state != NES_CM_STATE_SYN_SENT)
-			rem_ref_cm_node(cm_core, cm_node);
-			return 0;
+		if (process_options(cm_node, optionsloc, optionsize,
+			(u32)tcph->syn)) {
+			nes_debug(NES_DBG_CM, "%s: Node %p, Sending RESET\n",
+				__func__, cm_node);
+			if (passive)
+				passive_open_err(cm_node, skb, 0);
+			else
+				active_open_err(cm_node, skb, 0);
+			return 1;
 		}
-	} else if (tcph->syn)
-		cm_node->tcp_cntxt.mss = NES_CM_DEFAULT_MSS;
+	}
 
 	cm_node->tcp_cntxt.snd_wnd = ntohs(tcph->window) <<
 			cm_node->tcp_cntxt.snd_wscale;
 
-	if (cm_node->tcp_cntxt.snd_wnd > cm_node->tcp_cntxt.max_snd_wnd) {
+	if (cm_node->tcp_cntxt.snd_wnd > cm_node->tcp_cntxt.max_snd_wnd)
 		cm_node->tcp_cntxt.max_snd_wnd = cm_node->tcp_cntxt.snd_wnd;
-	}
+	return 0;
+}
 
-	if (tcph->ack) {
-		cm_node->tcp_cntxt.rem_ack_num = ntohl(tcph->ack_seq);
-		switch (cm_node->state) {
-			case NES_CM_STATE_SYN_RCVD:
-			case NES_CM_STATE_SYN_SENT:
-				/* read and stash current sequence number */
-				if (cm_node->tcp_cntxt.rem_ack_num != cm_node->tcp_cntxt.loc_seq_num) {
-					nes_debug(NES_DBG_CM, "ERROR - cm_node->tcp_cntxt.rem_ack_num !="
-							" cm_node->tcp_cntxt.loc_seq_num\n");
-					send_reset(cm_node);
-					return 0;
-				}
-				if (cm_node->state == NES_CM_STATE_SYN_SENT)
-					cm_node->state = NES_CM_STATE_ONE_SIDE_ESTABLISHED;
-				else {
-						cm_node->state = NES_CM_STATE_ESTABLISHED;
-				}
-				break;
-			case NES_CM_STATE_LAST_ACK:
-				cm_node->state = NES_CM_STATE_CLOSED;
-				break;
-			case NES_CM_STATE_FIN_WAIT1:
-				cm_node->state = NES_CM_STATE_FIN_WAIT2;
-				break;
-			case NES_CM_STATE_CLOSING:
-				cm_node->state = NES_CM_STATE_TIME_WAIT;
-				/* need to schedule this to happen in 2MSL timeouts */
-				cm_node->state = NES_CM_STATE_CLOSED;
-				break;
-			case NES_CM_STATE_ONE_SIDE_ESTABLISHED:
-			case NES_CM_STATE_ESTABLISHED:
-			case NES_CM_STATE_MPAREQ_SENT:
-			case NES_CM_STATE_CLOSE_WAIT:
-			case NES_CM_STATE_TIME_WAIT:
-			case NES_CM_STATE_CLOSED:
-				break;
-			case NES_CM_STATE_LISTENING:
-				nes_debug(NES_DBG_CM, "Received an ACK on a listening port (SYN %d)\n", tcph->syn);
-				cm_node->tcp_cntxt.loc_seq_num = ntohl(tcph->ack_seq);
-				send_reset(cm_node);
-				/* send_reset bumps refcount, this should have been a new node */
-				rem_ref_cm_node(cm_core, cm_node);
-				return -1;
-				break;
-			case NES_CM_STATE_TSA:
-				nes_debug(NES_DBG_CM, "Received a packet with the ack bit set while in TSA state\n");
-				break;
-			case NES_CM_STATE_UNKNOWN:
-			case NES_CM_STATE_INITED:
-			case NES_CM_STATE_ACCEPTING:
-			case NES_CM_STATE_FIN_WAIT2:
-			default:
-				nes_debug(NES_DBG_CM, "Received ack from unknown state: %x\n",
-						cm_node->state);
-				send_reset(cm_node);
-				break;
-		}
-	}
+/*
+ * active_open_err() will send reset() if flag set..
+ * It will also send ABORT event.
+ */
 
-	if (tcph->syn) {
-		if (cm_node->state == NES_CM_STATE_LISTENING) {
-			/* do not exceed backlog */
-			atomic_inc(&cm_node->listener->pend_accepts_cnt);
-			if (atomic_read(&cm_node->listener->pend_accepts_cnt) >
-					cm_node->listener->backlog) {
-				nes_debug(NES_DBG_CM, "drop syn due to backlog pressure \n");
-				cm_backlog_drops++;
-				atomic_dec(&cm_node->listener->pend_accepts_cnt);
-				rem_ref_cm_node(cm_core, cm_node);
-				return 0;
-			}
-			cm_node->accept_pend = 1;
+static void active_open_err(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	int reset)
+{
+	cleanup_retrans_entry(cm_node);
+	if (reset) {
+		nes_debug(NES_DBG_CM, "ERROR active err called for cm_node=%p, "
+				"state=%d\n", cm_node, cm_node->state);
+		add_ref_cm_node(cm_node);
+		send_reset(cm_node, skb);
+	} else
+		dev_kfree_skb_any(skb);
 
-		}
-		if (datasize == 0)
-			cm_node->tcp_cntxt.rcv_nxt ++;
+	cm_node->state = NES_CM_STATE_CLOSED;
+	create_event(cm_node, NES_CM_EVENT_ABORTED);
+}
 
-		if (cm_node->state == NES_CM_STATE_LISTENING) {
-			cm_node->state = NES_CM_STATE_SYN_RCVD;
-			send_syn(cm_node, 1);
-		}
-		if (cm_node->state == NES_CM_STATE_ONE_SIDE_ESTABLISHED) {
-			cm_node->state = NES_CM_STATE_ESTABLISHED;
-			/* send final handshake ACK */
-			ret = send_ack(cm_node);
-			if (ret < 0)
-				return ret;
+/*
+ * passive_open_err() will either do a reset() or will free up the skb and
+ * remove the cm_node.
+ */
 
-				cm_node->state = NES_CM_STATE_MPAREQ_SENT;
-				ret = send_mpa_request(cm_node);
-				if (ret < 0)
-					return ret;
-		}
+static void passive_open_err(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	int reset)
+{
+	cleanup_retrans_entry(cm_node);
+	cm_node->state = NES_CM_STATE_CLOSED;
+	if (reset) {
+		nes_debug(NES_DBG_CM, "passive_open_err sending RST for "
+			"cm_node=%p state =%d\n", cm_node, cm_node->state);
+		send_reset(cm_node, skb);
+	} else {
+		dev_kfree_skb_any(skb);
+		rem_ref_cm_node(cm_node->cm_core, cm_node);
 	}
+}
 
-	if (tcph->fin) {
-		cm_node->tcp_cntxt.rcv_nxt++;
-		switch (cm_node->state) {
-			case NES_CM_STATE_SYN_RCVD:
-			case NES_CM_STATE_SYN_SENT:
-			case NES_CM_STATE_ONE_SIDE_ESTABLISHED:
-			case NES_CM_STATE_ESTABLISHED:
-			case NES_CM_STATE_ACCEPTING:
-			case NES_CM_STATE_MPAREQ_SENT:
-				cm_node->state = NES_CM_STATE_CLOSE_WAIT;
-				cm_node->state = NES_CM_STATE_LAST_ACK;
-				ret = send_fin(cm_node, NULL);
-				break;
-			case NES_CM_STATE_FIN_WAIT1:
-				cm_node->state = NES_CM_STATE_CLOSING;
-				ret = send_ack(cm_node);
-				break;
-			case NES_CM_STATE_FIN_WAIT2:
-				cm_node->state = NES_CM_STATE_TIME_WAIT;
-				cm_node->tcp_cntxt.loc_seq_num ++;
-				ret = send_ack(cm_node);
-				/* need to schedule this to happen in 2MSL timeouts */
-				cm_node->state = NES_CM_STATE_CLOSED;
-				break;
-			case NES_CM_STATE_CLOSE_WAIT:
-			case NES_CM_STATE_LAST_ACK:
-			case NES_CM_STATE_CLOSING:
-			case NES_CM_STATE_TSA:
-			default:
-				nes_debug(NES_DBG_CM, "Received a fin while in %x state\n",
-						cm_node->state);
-				ret = -EINVAL;
-				break;
-		}
+/*
+ * free_retrans_entry() routines assumes that the retrans_list_lock has
+ * been acquired before calling.
+ */
+static void free_retrans_entry(struct nes_cm_node *cm_node)
+{
+	struct nes_timer_entry *send_entry;
+	send_entry = cm_node->send_entry;
+	if (send_entry) {
+		cm_node->send_entry = NULL;
+		dev_kfree_skb_any(send_entry->skb);
+		kfree(send_entry);
+		rem_ref_cm_node(cm_node->cm_core, cm_node);
 	}
+}
 
-	if (datasize) {
-		u8 *dataloc = skb->data;
-		/* figure out what state we are in and handle transition to next state */
-		switch (cm_node->state) {
-			case NES_CM_STATE_LISTENING:
-			case NES_CM_STATE_SYN_RCVD:
-			case NES_CM_STATE_SYN_SENT:
-			case NES_CM_STATE_FIN_WAIT1:
-			case NES_CM_STATE_FIN_WAIT2:
-			case NES_CM_STATE_CLOSE_WAIT:
-			case NES_CM_STATE_LAST_ACK:
-			case NES_CM_STATE_CLOSING:
-				break;
-			case  NES_CM_STATE_MPAREQ_SENT:
-				/* recv the mpa res frame, ret=frame len (incl priv data) */
-				ret = parse_mpa(cm_node, dataloc, datasize);
-				if (ret < 0)
-					break;
-				/* set the req frame payload len in skb */
-				/* we are done handling this state, set node to a TSA state */
-				cm_node->state = NES_CM_STATE_TSA;
-				send_ack(cm_node);
-				create_event(cm_node, NES_CM_EVENT_CONNECTED);
-				break;
-
-			case  NES_CM_STATE_ESTABLISHED:
-				/* we are expecting an MPA req frame */
-				ret = parse_mpa(cm_node, dataloc, datasize);
-				if (ret < 0) {
-					break;
-				}
-				cm_node->state = NES_CM_STATE_TSA;
-				send_ack(cm_node);
-				/* we got a valid MPA request, create an event */
-				create_event(cm_node, NES_CM_EVENT_MPA_REQ);
-				break;
-			case  NES_CM_STATE_TSA:
-				handle_exception_pkt(cm_node, skb);
-				break;
-			case NES_CM_STATE_UNKNOWN:
-			case NES_CM_STATE_INITED:
-			default:
-				ret = -1;
-		}
-	}
+static void cleanup_retrans_entry(struct nes_cm_node *cm_node)
+{
+	unsigned long flags;
 
-	return ret;
+	spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
+	free_retrans_entry(cm_node);
+	spin_unlock_irqrestore(&cm_node->retrans_list_lock, flags);
 }
 
+/**
+ * process_packet
+ * Returns skb if to be freed, else it will return NULL if already used..
+ */
+static void process_packet(struct nes_cm_node *cm_node, struct sk_buff *skb,
+	struct nes_cm_core *cm_core)
+{
+	enum nes_tcpip_pkt_type	pkt_type = NES_PKT_TYPE_UNKNOWN;
+	struct tcphdr *tcph = tcp_hdr(skb);
+	skb_pull(skb, ip_hdr(skb)->ihl << 2);
+
+	nes_debug(NES_DBG_CM, "process_packet: cm_node=%p state =%d syn=%d "
+		"ack=%d rst=%d fin=%d\n", cm_node, cm_node->state, tcph->syn,
+		tcph->ack, tcph->rst, tcph->fin);
+
+	if (tcph->rst)
+		pkt_type = NES_PKT_TYPE_RST;
+	else if (tcph->syn) {
+		pkt_type = NES_PKT_TYPE_SYN;
+		if (tcph->ack)
+			pkt_type = NES_PKT_TYPE_SYNACK;
+	} else if (tcph->fin)
+		pkt_type = NES_PKT_TYPE_FIN;
+	else if (tcph->ack)
+		pkt_type = NES_PKT_TYPE_ACK;
+
+	switch (pkt_type) {
+	case NES_PKT_TYPE_SYN:
+		handle_syn_pkt(cm_node, skb, tcph);
+		break;
+	case NES_PKT_TYPE_SYNACK:
+		handle_synack_pkt(cm_node, skb, tcph);
+		break;
+	case NES_PKT_TYPE_ACK:
+		handle_ack_pkt(cm_node, skb, tcph);
+		break;
+	case NES_PKT_TYPE_RST:
+		handle_rst_pkt(cm_node, skb, tcph);
+		break;
+	case NES_PKT_TYPE_FIN:
+		handle_fin_pkt(cm_node, skb, tcph);
+		break;
+	default:
+		drop_packet(skb);
+		break;
+	}
+}
 
 /**
  * mini_cm_listen - create a listen node with params
  */
 static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *cm_core,
-		struct nes_vnic *nesvnic, struct nes_cm_info *cm_info)
+	struct nes_vnic *nesvnic, struct nes_cm_info *cm_info)
 {
 	struct nes_cm_listener *listener;
 	unsigned long flags;
@@ -1644,37 +1826,36 @@ static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *cm_core,
 /**
  * mini_cm_connect - make a connection node with params
  */
-static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
-					   struct nes_vnic *nesvnic,
-					   struct ietf_mpa_frame *mpa_frame,
-					   struct nes_cm_info *cm_info)
+struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
+	struct nes_vnic *nesvnic, u16 private_data_len,
+	void *private_data, struct nes_cm_info *cm_info)
 {
 	int ret = 0;
 	struct nes_cm_node *cm_node;
 	struct nes_cm_listener *loopbackremotelistener;
 	struct nes_cm_node *loopbackremotenode;
 	struct nes_cm_info loopback_cm_info;
-
-	u16 mpa_frame_size = sizeof(struct ietf_mpa_frame) +
-			ntohs(mpa_frame->priv_data_len);
-
-	cm_info->loc_addr = htonl(cm_info->loc_addr);
-	cm_info->rem_addr = htonl(cm_info->rem_addr);
-	cm_info->loc_port = htons(cm_info->loc_port);
-	cm_info->rem_port = htons(cm_info->rem_port);
+	u16 mpa_frame_size = sizeof(struct ietf_mpa_frame) + private_data_len;
+	struct ietf_mpa_frame *mpa_frame = NULL;
 
 	/* create a CM connection node */
 	cm_node = make_cm_node(cm_core, nesvnic, cm_info, NULL);
 	if (!cm_node)
 		return NULL;
+	mpa_frame = &cm_node->mpa_frame;
+	strcpy(mpa_frame->key, IEFT_MPA_KEY_REQ);
+	mpa_frame->flags = IETF_MPA_FLAGS_CRC;
+	mpa_frame->rev =  IETF_MPA_VERSION;
+	mpa_frame->priv_data_len = htons(private_data_len);
 
 	/* set our node side to client (active) side */
 	cm_node->tcp_cntxt.client = 1;
 	cm_node->tcp_cntxt.rcv_wscale = NES_CM_DEFAULT_RCV_WND_SCALE;
 
 	if (cm_info->loc_addr == cm_info->rem_addr) {
-		loopbackremotelistener = find_listener(cm_core, cm_node->rem_addr,
-				cm_node->rem_port, NES_CM_LISTENER_ACTIVE_STATE);
+		loopbackremotelistener = find_listener(cm_core,
+				ntohl(nesvnic->local_ipaddr), cm_node->rem_port,
+				NES_CM_LISTENER_ACTIVE_STATE);
 		if (loopbackremotelistener == NULL) {
 			create_event(cm_node, NES_CM_EVENT_ABORTED);
 		} else {
@@ -1683,26 +1864,35 @@ static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
 			loopback_cm_info.loc_port = cm_info->rem_port;
 			loopback_cm_info.rem_port = cm_info->loc_port;
 			loopback_cm_info.cm_id = loopbackremotelistener->cm_id;
-			loopbackremotenode = make_cm_node(cm_core, nesvnic, &loopback_cm_info,
-					loopbackremotelistener);
+			loopbackremotenode = make_cm_node(cm_core, nesvnic,
+				&loopback_cm_info, loopbackremotelistener);
 			loopbackremotenode->loopbackpartner = cm_node;
-			loopbackremotenode->tcp_cntxt.rcv_wscale = NES_CM_DEFAULT_RCV_WND_SCALE;
+			loopbackremotenode->tcp_cntxt.rcv_wscale =
+				NES_CM_DEFAULT_RCV_WND_SCALE;
 			cm_node->loopbackpartner = loopbackremotenode;
-			memcpy(loopbackremotenode->mpa_frame_buf, &mpa_frame->priv_data,
-					mpa_frame_size);
-			loopbackremotenode->mpa_frame_size = mpa_frame_size -
-					sizeof(struct ietf_mpa_frame);
+			memcpy(loopbackremotenode->mpa_frame_buf, private_data,
+				private_data_len);
+			loopbackremotenode->mpa_frame_size = private_data_len;
 
-			/* we are done handling this state, set node to a TSA state */
+			/* we are done handling this state. */
+			/* set node to a TSA state */
 			cm_node->state = NES_CM_STATE_TSA;
-			cm_node->tcp_cntxt.rcv_nxt = loopbackremotenode->tcp_cntxt.loc_seq_num;
-			loopbackremotenode->tcp_cntxt.rcv_nxt = cm_node->tcp_cntxt.loc_seq_num;
-			cm_node->tcp_cntxt.max_snd_wnd = loopbackremotenode->tcp_cntxt.rcv_wnd;
-			loopbackremotenode->tcp_cntxt.max_snd_wnd = cm_node->tcp_cntxt.rcv_wnd;
-			cm_node->tcp_cntxt.snd_wnd = loopbackremotenode->tcp_cntxt.rcv_wnd;
-			loopbackremotenode->tcp_cntxt.snd_wnd = cm_node->tcp_cntxt.rcv_wnd;
-			cm_node->tcp_cntxt.snd_wscale = loopbackremotenode->tcp_cntxt.rcv_wscale;
-			loopbackremotenode->tcp_cntxt.snd_wscale = cm_node->tcp_cntxt.rcv_wscale;
+			cm_node->tcp_cntxt.rcv_nxt =
+				loopbackremotenode->tcp_cntxt.loc_seq_num;
+			loopbackremotenode->tcp_cntxt.rcv_nxt =
+				cm_node->tcp_cntxt.loc_seq_num;
+			cm_node->tcp_cntxt.max_snd_wnd =
+				loopbackremotenode->tcp_cntxt.rcv_wnd;
+			loopbackremotenode->tcp_cntxt.max_snd_wnd =
+				cm_node->tcp_cntxt.rcv_wnd;
+			cm_node->tcp_cntxt.snd_wnd =
+				loopbackremotenode->tcp_cntxt.rcv_wnd;
+			loopbackremotenode->tcp_cntxt.snd_wnd =
+				cm_node->tcp_cntxt.rcv_wnd;
+			cm_node->tcp_cntxt.snd_wscale =
+				loopbackremotenode->tcp_cntxt.rcv_wscale;
+			loopbackremotenode->tcp_cntxt.snd_wscale =
+				cm_node->tcp_cntxt.rcv_wscale;
 
 			create_event(loopbackremotenode, NES_CM_EVENT_MPA_REQ);
 		}
@@ -1712,16 +1902,29 @@ static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
 	/* set our node side to client (active) side */
 	cm_node->tcp_cntxt.client = 1;
 	/* init our MPA frame ptr */
-	memcpy(&cm_node->mpa_frame, mpa_frame, mpa_frame_size);
+	memcpy(mpa_frame->priv_data, private_data, private_data_len);
+
 	cm_node->mpa_frame_size = mpa_frame_size;
 
 	/* send a syn and goto syn sent state */
 	cm_node->state = NES_CM_STATE_SYN_SENT;
-	ret = send_syn(cm_node, 0);
+	ret = send_syn(cm_node, 0, NULL);
+
+	if (ret) {
+		/* error in sending the syn free up the cm_node struct */
+		nes_debug(NES_DBG_CM, "Api - connect() FAILED: dest "
+			"addr=0x%08X, port=0x%04x, cm_node=%p, cm_id = %p.\n",
+			cm_node->rem_addr, cm_node->rem_port, cm_node,
+			cm_node->cm_id);
+		rem_ref_cm_node(cm_node->cm_core, cm_node);
+		cm_node = NULL;
+	}
 
-	nes_debug(NES_DBG_CM, "Api - connect(): dest addr=0x%08X, port=0x%04x,"
-			" cm_node=%p, cm_id = %p.\n",
-			cm_node->rem_addr, cm_node->rem_port, cm_node, cm_node->cm_id);
+	if (cm_node)
+		nes_debug(NES_DBG_CM, "Api - connect(): dest addr=0x%08X,"
+			"port=0x%04x, cm_node=%p, cm_id = %p.\n",
+			cm_node->rem_addr, cm_node->rem_port, cm_node,
+			cm_node->cm_id);
 
 	return cm_node;
 }
@@ -1731,8 +1934,8 @@ static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
  * mini_cm_accept - accept a connection
  * This function is never called
  */
-static int mini_cm_accept(struct nes_cm_core *cm_core, struct ietf_mpa_frame *mpa_frame,
-			  struct nes_cm_node *cm_node)
+static int mini_cm_accept(struct nes_cm_core *cm_core,
+	struct ietf_mpa_frame *mpa_frame, struct nes_cm_node *cm_node)
 {
 	return 0;
 }
@@ -1742,32 +1945,26 @@ static int mini_cm_accept(struct nes_cm_core *cm_core, struct ietf_mpa_frame *mp
  * mini_cm_reject - reject and teardown a connection
  */
 static int mini_cm_reject(struct nes_cm_core *cm_core,
-			  struct ietf_mpa_frame *mpa_frame,
-			  struct nes_cm_node *cm_node)
+	struct ietf_mpa_frame *mpa_frame, struct nes_cm_node *cm_node)
 {
 	int ret = 0;
-	struct sk_buff *skb;
-	u16 mpa_frame_size = sizeof(struct ietf_mpa_frame) +
-			ntohs(mpa_frame->priv_data_len);
 
-	skb = get_free_pkt(cm_node);
-	if (!skb) {
-		nes_debug(NES_DBG_CM, "Failed to get a Free pkt\n");
-		return -1;
-	}
-
-	/* send an MPA Request frame */
-	form_cm_frame(skb, cm_node, NULL, 0, mpa_frame, mpa_frame_size, SET_ACK | SET_FIN);
-	ret = schedule_nes_timer(cm_node, skb, NES_TIMER_TYPE_SEND, 1, 0);
+	nes_debug(NES_DBG_CM, "%s cm_node=%p type=%d state=%d\n",
+		__func__, cm_node, cm_node->tcp_cntxt.client, cm_node->state);
 
+	if (cm_node->tcp_cntxt.client)
+		return ret;
+	cleanup_retrans_entry(cm_node);
 	cm_node->state = NES_CM_STATE_CLOSED;
 	ret = send_fin(cm_node, NULL);
 
-	if (ret < 0) {
-		printk(KERN_INFO PFX "failed to send MPA Reply (reject)\n");
-		return ret;
+	if (cm_node->accept_pend) {
+		BUG_ON(!cm_node->listener);
+		atomic_dec(&cm_node->listener->pend_accepts_cnt);
+		BUG_ON(atomic_read(&cm_node->listener->pend_accepts_cnt) < 0);
 	}
 
+	ret = send_reset(cm_node, NULL);
 	return ret;
 }
 
@@ -1783,35 +1980,39 @@ static int mini_cm_close(struct nes_cm_core *cm_core, struct nes_cm_node *cm_nod
 		return -EINVAL;
 
 	switch (cm_node->state) {
-		/* if passed in node is null, create a reference key node for node search */
-		/* check if we found an owner node for this pkt */
-		case NES_CM_STATE_SYN_RCVD:
-		case NES_CM_STATE_SYN_SENT:
-		case NES_CM_STATE_ONE_SIDE_ESTABLISHED:
-		case NES_CM_STATE_ESTABLISHED:
-		case NES_CM_STATE_ACCEPTING:
-		case NES_CM_STATE_MPAREQ_SENT:
-			cm_node->state = NES_CM_STATE_FIN_WAIT1;
-			send_fin(cm_node, NULL);
-			break;
-		case NES_CM_STATE_CLOSE_WAIT:
-			cm_node->state = NES_CM_STATE_LAST_ACK;
-			send_fin(cm_node, NULL);
-			break;
-		case NES_CM_STATE_FIN_WAIT1:
-		case NES_CM_STATE_FIN_WAIT2:
-		case NES_CM_STATE_LAST_ACK:
-		case NES_CM_STATE_TIME_WAIT:
-		case NES_CM_STATE_CLOSING:
-			ret = -1;
-			break;
-		case NES_CM_STATE_LISTENING:
-		case NES_CM_STATE_UNKNOWN:
-		case NES_CM_STATE_INITED:
-		case NES_CM_STATE_CLOSED:
-		case NES_CM_STATE_TSA:
-			ret = rem_ref_cm_node(cm_core, cm_node);
-			break;
+	case NES_CM_STATE_SYN_RCVD:
+	case NES_CM_STATE_SYN_SENT:
+	case NES_CM_STATE_ONE_SIDE_ESTABLISHED:
+	case NES_CM_STATE_ESTABLISHED:
+	case NES_CM_STATE_ACCEPTING:
+	case NES_CM_STATE_MPAREQ_SENT:
+	case NES_CM_STATE_MPAREQ_RCVD:
+		cleanup_retrans_entry(cm_node);
+		send_reset(cm_node, NULL);
+		break;
+	case NES_CM_STATE_CLOSE_WAIT:
+		cm_node->state = NES_CM_STATE_LAST_ACK;
+		send_fin(cm_node, NULL);
+		break;
+	case NES_CM_STATE_FIN_WAIT1:
+	case NES_CM_STATE_FIN_WAIT2:
+	case NES_CM_STATE_LAST_ACK:
+	case NES_CM_STATE_TIME_WAIT:
+	case NES_CM_STATE_CLOSING:
+		ret = -1;
+		break;
+	case NES_CM_STATE_LISTENING:
+	case NES_CM_STATE_UNKNOWN:
+	case NES_CM_STATE_INITED:
+	case NES_CM_STATE_CLOSED:
+		ret = rem_ref_cm_node(cm_core, cm_node);
+		break;
+	case NES_CM_STATE_TSA:
+		if (cm_node->send_entry)
+			printk(KERN_ERR "ERROR Close got called from STATE_TSA "
+				"send_entry=%p\n", cm_node->send_entry);
+		ret = rem_ref_cm_node(cm_core, cm_node);
+		break;
 	}
 	cm_node->cm_id = NULL;
 	return ret;
@@ -1822,25 +2023,30 @@ static int mini_cm_close(struct nes_cm_core *cm_core, struct nes_cm_node *cm_nod
  * recv_pkt - recv an ETHERNET packet, and process it through CM
  * node state machine
  */
-static int mini_cm_recv_pkt(struct nes_cm_core *cm_core, struct nes_vnic *nesvnic,
-			    struct sk_buff *skb)
+static void mini_cm_recv_pkt(struct nes_cm_core *cm_core,
+	struct nes_vnic *nesvnic, struct sk_buff *skb)
 {
 	struct nes_cm_node *cm_node = NULL;
 	struct nes_cm_listener *listener = NULL;
 	struct iphdr *iph;
 	struct tcphdr *tcph;
 	struct nes_cm_info nfo;
-	int ret = 0;
 
-	if (!skb || skb->len < sizeof(struct iphdr) + sizeof(struct tcphdr)) {
-		ret = -EINVAL;
-		goto out;
+	if (!skb)
+		return;
+	if (skb->len < sizeof(struct iphdr) + sizeof(struct tcphdr)) {
+		dev_kfree_skb_any(skb);
+		return;
 	}
 
 	iph = (struct iphdr *)skb->data;
 	tcph = (struct tcphdr *)(skb->data + sizeof(struct iphdr));
 	skb_reset_network_header(skb);
 	skb_set_transport_header(skb, sizeof(*tcph));
+	if (!tcph) {
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	skb->len = ntohs(iph->tot_len);
 
 	nfo.loc_addr = ntohl(iph->daddr);
@@ -1853,61 +2059,60 @@ static int mini_cm_recv_pkt(struct nes_cm_core *cm_core, struct nes_vnic *nesvni
 		  NIPQUAD(iph->daddr), tcph->dest,
 		  NIPQUAD(iph->saddr), tcph->source);
 
-	/* note: this call is going to increment cm_node ref count */
-	cm_node = find_node(cm_core,
+	do {
+		cm_node = find_node(cm_core,
 			nfo.rem_port, nfo.rem_addr,
 			nfo.loc_port, nfo.loc_addr);
 
-	if (!cm_node) {
-		listener = find_listener(cm_core, nfo.loc_addr, nfo.loc_port,
-				NES_CM_LISTENER_ACTIVE_STATE);
-		if (listener) {
-			nfo.cm_id = listener->cm_id;
-			nfo.conn_type = listener->conn_type;
-		} else {
-			nfo.cm_id = NULL;
-			nfo.conn_type = 0;
-		}
-
-		cm_node = make_cm_node(cm_core, nesvnic, &nfo, listener);
 		if (!cm_node) {
-			nes_debug(NES_DBG_CM, "Unable to allocate node\n");
+			/* Only type of packet accepted are for */
+			/* the PASSIVE open (syn only) */
+			if ((!tcph->syn) || (tcph->ack)) {
+				cm_packets_dropped++;
+				break;
+			}
+			listener = find_listener(cm_core, nfo.loc_addr,
+				nfo.loc_port,
+				NES_CM_LISTENER_ACTIVE_STATE);
 			if (listener) {
-				nes_debug(NES_DBG_CM, "unable to allocate node and decrementing listener refcount\n");
+				nfo.cm_id = listener->cm_id;
+				nfo.conn_type = listener->conn_type;
+			} else {
+				nes_debug(NES_DBG_CM, "Unable to find listener "
+					"for the pkt\n");
+				cm_packets_dropped++;
+				dev_kfree_skb_any(skb);
+				break;
+			}
+
+			cm_node = make_cm_node(cm_core, nesvnic, &nfo,
+				listener);
+			if (!cm_node) {
+				nes_debug(NES_DBG_CM, "Unable to allocate "
+					"node\n");
+				cm_packets_dropped++;
 				atomic_dec(&listener->ref_count);
+				dev_kfree_skb_any(skb);
+				break;
 			}
-			ret = -1;
-			goto out;
-		}
-		if (!listener) {
-			nes_debug(NES_DBG_CM, "Packet found for unknown port %x refcnt=%d\n",
-					nfo.loc_port, atomic_read(&cm_node->ref_count));
-			if (!tcph->rst) {
-				nes_debug(NES_DBG_CM, "Packet found for unknown port=%d"
-						" rem_port=%d refcnt=%d\n",
-						nfo.loc_port, nfo.rem_port, atomic_read(&cm_node->ref_count));
-
-				cm_node->tcp_cntxt.rcv_nxt = ntohl(tcph->seq);
-				cm_node->tcp_cntxt.loc_seq_num = ntohl(tcph->ack_seq);
-				send_reset(cm_node);
+			if (!tcph->rst && !tcph->fin) {
+				cm_node->state = NES_CM_STATE_LISTENING;
+			} else {
+				cm_packets_dropped++;
+				rem_ref_cm_node(cm_core, cm_node);
+				dev_kfree_skb_any(skb);
+				break;
 			}
+			add_ref_cm_node(cm_node);
+		} else if (cm_node->state == NES_CM_STATE_TSA) {
 			rem_ref_cm_node(cm_core, cm_node);
-			ret = -1;
-			goto out;
+			atomic_inc(&cm_accel_dropped_pkts);
+			dev_kfree_skb_any(skb);
+			break;
 		}
-		add_ref_cm_node(cm_node);
-		cm_node->state = NES_CM_STATE_LISTENING;
-	}
-
-	nes_debug(NES_DBG_CM, "Processing Packet for node %p, data = (%p):\n",
-			cm_node, skb->data);
-	process_packet(cm_node, skb, cm_core);
-
-	rem_ref_cm_node(cm_core, cm_node);
-	out:
-	if (skb)
-		dev_kfree_skb_any(skb);
-	return ret;
+		process_packet(cm_node, skb, cm_core);
+		rem_ref_cm_node(cm_core, cm_node);
+	} while (0);
 }
 
 
@@ -2107,15 +2312,12 @@ int nes_cm_disconn(struct nes_qp *nesqp)
 	if (nesqp->disconn_pending == 0) {
 		nesqp->disconn_pending++;
 		spin_unlock_irqrestore(&nesqp->lock, flags);
-		/* nes_add_ref(&nesqp->ibqp); */
 		/* init our disconnect work element, to */
 		INIT_WORK(&nesqp->disconn_work, nes_disconnect_worker);
 
 		queue_work(g_cm_core->disconn_wq, &nesqp->disconn_work);
-	} else {
+	} else
 		spin_unlock_irqrestore(&nesqp->lock, flags);
-		nes_rem_ref(&nesqp->ibqp);
-	}
 
 	return 0;
 }
@@ -2161,7 +2363,6 @@ static int nes_cm_disconn_true(struct nes_qp *nesqp)
 		nes_debug(NES_DBG_CM, "QP%u disconnect_worker cmid is NULL\n",
 				nesqp->hwqp.qp_id);
 		spin_unlock_irqrestore(&nesqp->lock, flags);
-		nes_rem_ref(&nesqp->ibqp);
 		return -1;
 	}
 
@@ -2182,30 +2383,31 @@ static int nes_cm_disconn_true(struct nes_qp *nesqp)
 			atomic_inc(&cm_disconnects);
 			cm_event.event = IW_CM_EVENT_DISCONNECT;
 			if (last_ae == NES_AEQE_AEID_LLP_CONNECTION_RESET) {
-				issued_disconnect_reset = 1;
 				cm_event.status = IW_CM_EVENT_STATUS_RESET;
-				nes_debug(NES_DBG_CM, "Generating a CM Disconnect Event (status reset) for "
-						" QP%u, cm_id = %p. \n",
-						nesqp->hwqp.qp_id, cm_id);
-			} else {
+				nes_debug(NES_DBG_CM, "Generating a CM "
+					"Disconnect Event (status reset) for "
+					"QP%u, cm_id = %p. \n",
+					nesqp->hwqp.qp_id, cm_id);
+			} else
 				cm_event.status = IW_CM_EVENT_STATUS_OK;
-			}
 
 			cm_event.local_addr = cm_id->local_addr;
 			cm_event.remote_addr = cm_id->remote_addr;
 			cm_event.private_data = NULL;
 			cm_event.private_data_len = 0;
 
-			nes_debug(NES_DBG_CM, "Generating a CM Disconnect Event for "
-					" QP%u, SQ Head = %u, SQ Tail = %u. cm_id = %p, refcount = %u.\n",
-					nesqp->hwqp.qp_id,
-					nesqp->hwqp.sq_head, nesqp->hwqp.sq_tail, cm_id,
-					atomic_read(&nesqp->refcount));
+			nes_debug(NES_DBG_CM, "Generating a CM Disconnect Event"
+				" for  QP%u, SQ Head = %u, SQ Tail = %u. "
+				"cm_id = %p, refcount = %u.\n",
+				nesqp->hwqp.qp_id, nesqp->hwqp.sq_head,
+				nesqp->hwqp.sq_tail, cm_id,
+				atomic_read(&nesqp->refcount));
 
 			spin_unlock_irqrestore(&nesqp->lock, flags);
 			ret = cm_id->event_handler(cm_id, &cm_event);
 			if (ret)
-				nes_debug(NES_DBG_CM, "OFA CM event_handler returned, ret=%d\n", ret);
+				nes_debug(NES_DBG_CM, "OFA CM event_handler "
+					"returned, ret=%d\n", ret);
 			spin_lock_irqsave(&nesqp->lock, flags);
 		}
 
@@ -2247,31 +2449,24 @@ static int nes_cm_disconn_true(struct nes_qp *nesqp)
 			if (nesqp->flush_issued == 0) {
 				nesqp->flush_issued = 1;
 				spin_unlock_irqrestore(&nesqp->lock, flags);
-				flush_wqes(nesvnic->nesdev, nesqp, NES_CQP_FLUSH_RQ, 1);
-			} else {
+				flush_wqes(nesvnic->nesdev, nesqp,
+					NES_CQP_FLUSH_RQ, 1);
+			} else
 				spin_unlock_irqrestore(&nesqp->lock, flags);
-			}
-
-			/* This reference is from either ModifyQP or the AE processing,
-					there is still a race here with modifyqp */
-			nes_rem_ref(&nesqp->ibqp);
-
 		} else {
 			cm_id = nesqp->cm_id;
 			spin_unlock_irqrestore(&nesqp->lock, flags);
 			/* check to see if the inbound reset beat the outbound reset */
 			if ((!cm_id) && (last_ae==NES_AEQE_AEID_RESET_SENT)) {
-				nes_debug(NES_DBG_CM, "QP%u: Decing refcount due to inbound reset"
-						" beating the outbound reset.\n",
-						nesqp->hwqp.qp_id);
-				nes_rem_ref(&nesqp->ibqp);
+				nes_debug(NES_DBG_CM, "QP%u: Decing refcount "
+					"due to inbound reset beating the "
+					"outbound reset.\n", nesqp->hwqp.qp_id);
 			}
 		}
 	} else {
 		nesqp->disconn_pending = 0;
 		spin_unlock_irqrestore(&nesqp->lock, flags);
 	}
-	nes_rem_ref(&nesqp->ibqp);
 
 	return 0;
 }
@@ -2349,71 +2544,82 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	nesdev = nesvnic->nesdev;
 	adapter = nesdev->nesadapter;
 
-	nes_debug(NES_DBG_CM, "nesvnic=%p, netdev=%p, %s\n",
-			nesvnic, nesvnic->netdev, nesvnic->netdev->name);
-
-	/* since this is from a listen, we were able to put node handle into cm_id */
 	cm_node = (struct nes_cm_node *)cm_id->provider_data;
+	nes_debug(NES_DBG_CM, "nes_accept: cm_node= %p nesvnic=%p, netdev=%p,"
+		"%s\n", cm_node, nesvnic, nesvnic->netdev,
+		nesvnic->netdev->name);
 
 	/* associate the node with the QP */
 	nesqp->cm_node = (void *)cm_node;
+	cm_node->nesqp = nesqp;
+	nes_add_ref(&nesqp->ibqp);
 
-	nes_debug(NES_DBG_CM, "QP%u, cm_node=%p, jiffies = %lu\n",
-			nesqp->hwqp.qp_id, cm_node, jiffies);
+	nes_debug(NES_DBG_CM, "QP%u, cm_node=%p, jiffies = %lu listener = %p\n",
+		nesqp->hwqp.qp_id, cm_node, jiffies, cm_node->listener);
 	atomic_inc(&cm_accepts);
 
 	nes_debug(NES_DBG_CM, "netdev refcnt = %u.\n",
 			atomic_read(&nesvnic->netdev->refcnt));
 
-		/* allocate the ietf frame and space for private data */
-		nesqp->ietf_frame = pci_alloc_consistent(nesdev->pcidev,
-				sizeof(struct ietf_mpa_frame) + conn_param->private_data_len,
-				&nesqp->ietf_frame_pbase);
-
-		if (!nesqp->ietf_frame) {
-			nes_debug(NES_DBG_CM, "Unable to allocate memory for private data\n");
-			return -ENOMEM;
-		}
+	/* allocate the ietf frame and space for private data */
+	nesqp->ietf_frame = pci_alloc_consistent(nesdev->pcidev,
+		sizeof(struct ietf_mpa_frame) + conn_param->private_data_len,
+		&nesqp->ietf_frame_pbase);
 
+	if (!nesqp->ietf_frame) {
+		nes_debug(NES_DBG_CM, "Unable to allocate memory for private "
+			"data\n");
+		return -ENOMEM;
+	}
 
-		/* setup the MPA frame */
-		nesqp->private_data_len = conn_param->private_data_len;
-		memcpy(nesqp->ietf_frame->key, IEFT_MPA_KEY_REP, IETF_MPA_KEY_SIZE);
 
-		memcpy(nesqp->ietf_frame->priv_data, conn_param->private_data,
-				conn_param->private_data_len);
+	/* setup the MPA frame */
+	nesqp->private_data_len = conn_param->private_data_len;
+	memcpy(nesqp->ietf_frame->key, IEFT_MPA_KEY_REP, IETF_MPA_KEY_SIZE);
 
-		nesqp->ietf_frame->priv_data_len = cpu_to_be16(conn_param->private_data_len);
-		nesqp->ietf_frame->rev = mpa_version;
-		nesqp->ietf_frame->flags = IETF_MPA_FLAGS_CRC;
+	memcpy(nesqp->ietf_frame->priv_data, conn_param->private_data,
+			conn_param->private_data_len);
 
-		/* setup our first outgoing iWarp send WQE (the IETF frame response) */
-		wqe = &nesqp->hwqp.sq_vbase[0];
+	nesqp->ietf_frame->priv_data_len =
+		cpu_to_be16(conn_param->private_data_len);
+	nesqp->ietf_frame->rev = mpa_version;
+	nesqp->ietf_frame->flags = IETF_MPA_FLAGS_CRC;
 
-		if (cm_id->remote_addr.sin_addr.s_addr != cm_id->local_addr.sin_addr.s_addr) {
-			u64temp = (unsigned long)nesqp;
-			u64temp |= NES_SW_CONTEXT_ALIGN>>1;
-			set_wqe_64bit_value(wqe->wqe_words, NES_IWARP_SQ_WQE_COMP_CTX_LOW_IDX,
-					    u64temp);
-			wqe->wqe_words[NES_IWARP_SQ_WQE_MISC_IDX] =
-					cpu_to_le32(NES_IWARP_SQ_WQE_STREAMING | NES_IWARP_SQ_WQE_WRPDU);
-			wqe->wqe_words[NES_IWARP_SQ_WQE_TOTAL_PAYLOAD_IDX] =
-					cpu_to_le32(conn_param->private_data_len + sizeof(struct ietf_mpa_frame));
-			wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_LOW_IDX] =
-					cpu_to_le32((u32)nesqp->ietf_frame_pbase);
-			wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_HIGH_IDX] =
-					cpu_to_le32((u32)((u64)nesqp->ietf_frame_pbase >> 32));
-			wqe->wqe_words[NES_IWARP_SQ_WQE_LENGTH0_IDX] =
-					cpu_to_le32(conn_param->private_data_len + sizeof(struct ietf_mpa_frame));
-			wqe->wqe_words[NES_IWARP_SQ_WQE_STAG0_IDX] = 0;
-
-			nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32(
-					NES_QPCONTEXT_ORDIRD_LSMM_PRESENT | NES_QPCONTEXT_ORDIRD_WRPDU);
-		} else {
-			nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32((NES_QPCONTEXT_ORDIRD_LSMM_PRESENT |
-					NES_QPCONTEXT_ORDIRD_WRPDU | NES_QPCONTEXT_ORDIRD_ALSMM));
-		}
-		nesqp->skip_lsmm = 1;
+	/* setup our first outgoing iWarp send WQE (the IETF frame response) */
+	wqe = &nesqp->hwqp.sq_vbase[0];
+
+	if (cm_id->remote_addr.sin_addr.s_addr !=
+			cm_id->local_addr.sin_addr.s_addr) {
+		u64temp = (unsigned long)nesqp;
+		u64temp |= NES_SW_CONTEXT_ALIGN>>1;
+		set_wqe_64bit_value(wqe->wqe_words,
+			NES_IWARP_SQ_WQE_COMP_CTX_LOW_IDX,
+			u64temp);
+		wqe->wqe_words[NES_IWARP_SQ_WQE_MISC_IDX] =
+			cpu_to_le32(NES_IWARP_SQ_WQE_STREAMING |
+			NES_IWARP_SQ_WQE_WRPDU);
+		wqe->wqe_words[NES_IWARP_SQ_WQE_TOTAL_PAYLOAD_IDX] =
+			cpu_to_le32(conn_param->private_data_len +
+			sizeof(struct ietf_mpa_frame));
+		wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_LOW_IDX] =
+			cpu_to_le32((u32)nesqp->ietf_frame_pbase);
+		wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_HIGH_IDX] =
+			cpu_to_le32((u32)((u64)nesqp->ietf_frame_pbase >> 32));
+		wqe->wqe_words[NES_IWARP_SQ_WQE_LENGTH0_IDX] =
+			cpu_to_le32(conn_param->private_data_len +
+			sizeof(struct ietf_mpa_frame));
+		wqe->wqe_words[NES_IWARP_SQ_WQE_STAG0_IDX] = 0;
+
+		nesqp->nesqp_context->ird_ord_sizes |=
+			cpu_to_le32(NES_QPCONTEXT_ORDIRD_LSMM_PRESENT |
+			NES_QPCONTEXT_ORDIRD_WRPDU);
+	} else {
+		nesqp->nesqp_context->ird_ord_sizes |=
+			cpu_to_le32((NES_QPCONTEXT_ORDIRD_LSMM_PRESENT |
+			NES_QPCONTEXT_ORDIRD_WRPDU |
+			NES_QPCONTEXT_ORDIRD_ALSMM));
+	}
+	nesqp->skip_lsmm = 1;
 
 
 	/* Cache the cm_id in the qp */
@@ -2424,55 +2630,75 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	cm_id->provider_data = nesqp;
 	nesqp->active_conn   = 0;
 
+	if (cm_node->state == NES_CM_STATE_TSA)
+		nes_debug(NES_DBG_CM, "Already state = TSA for cm_node=%p\n",
+			cm_node);
+
 	nes_cm_init_tsa_conn(nesqp, cm_node);
 
-	nesqp->nesqp_context->tcpPorts[0] = cpu_to_le16(ntohs(cm_id->local_addr.sin_port));
-	nesqp->nesqp_context->tcpPorts[1] = cpu_to_le16(ntohs(cm_id->remote_addr.sin_port));
-	nesqp->nesqp_context->ip0 = cpu_to_le32(ntohl(cm_id->remote_addr.sin_addr.s_addr));
+	nesqp->nesqp_context->tcpPorts[0] =
+		cpu_to_le16(ntohs(cm_id->local_addr.sin_port));
+	nesqp->nesqp_context->tcpPorts[1] =
+		cpu_to_le16(ntohs(cm_id->remote_addr.sin_port));
+
+	if (ipv4_is_loopback(cm_id->remote_addr.sin_addr.s_addr))
+		nesqp->nesqp_context->ip0 =
+			cpu_to_le32(ntohl(nesvnic->local_ipaddr));
+	else
+		nesqp->nesqp_context->ip0 =
+			cpu_to_le32(ntohl(cm_id->remote_addr.sin_addr.s_addr));
 
 	nesqp->nesqp_context->misc2 |= cpu_to_le32(
-			(u32)PCI_FUNC(nesdev->pcidev->devfn) << NES_QPCONTEXT_MISC2_SRC_IP_SHIFT);
+			(u32)PCI_FUNC(nesdev->pcidev->devfn) <<
+			NES_QPCONTEXT_MISC2_SRC_IP_SHIFT);
 
-	nesqp->nesqp_context->arp_index_vlan |= cpu_to_le32(
-			nes_arp_table(nesdev, le32_to_cpu(nesqp->nesqp_context->ip0), NULL,
+	nesqp->nesqp_context->arp_index_vlan |=
+		cpu_to_le32(nes_arp_table(nesdev,
+			le32_to_cpu(nesqp->nesqp_context->ip0), NULL,
 			NES_ARP_RESOLVE) << 16);
 
 	nesqp->nesqp_context->ts_val_delta = cpu_to_le32(
-			jiffies - nes_read_indexed(nesdev, NES_IDX_TCP_NOW));
+		jiffies - nes_read_indexed(nesdev, NES_IDX_TCP_NOW));
 
 	nesqp->nesqp_context->ird_index = cpu_to_le32(nesqp->hwqp.qp_id);
 
 	nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32(
-			((u32)1 << NES_QPCONTEXT_ORDIRD_IWARP_MODE_SHIFT));
-	nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32((u32)conn_param->ord);
+		((u32)1 << NES_QPCONTEXT_ORDIRD_IWARP_MODE_SHIFT));
+	nesqp->nesqp_context->ird_ord_sizes |=
+		cpu_to_le32((u32)conn_param->ord);
 
 	memset(&nes_quad, 0, sizeof(nes_quad));
-	nes_quad.DstIpAdrIndex = cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
-	nes_quad.SrcIpadr      = cm_id->remote_addr.sin_addr.s_addr;
-	nes_quad.TcpPorts[0]   = cm_id->remote_addr.sin_port;
-	nes_quad.TcpPorts[1]   = cm_id->local_addr.sin_port;
+	nes_quad.DstIpAdrIndex =
+		cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
+	if (ipv4_is_loopback(cm_id->remote_addr.sin_addr.s_addr))
+		nes_quad.SrcIpadr = nesvnic->local_ipaddr;
+	else
+		nes_quad.SrcIpadr = cm_id->remote_addr.sin_addr.s_addr;
+	nes_quad.TcpPorts[0] = cm_id->remote_addr.sin_port;
+	nes_quad.TcpPorts[1] = cm_id->local_addr.sin_port;
 
 	/* Produce hash key */
 	crc_value = get_crc_value(&nes_quad);
 	nesqp->hte_index = cpu_to_be32(crc_value ^ 0xffffffff);
 	nes_debug(NES_DBG_CM, "HTE Index = 0x%08X, CRC = 0x%08X\n",
-			nesqp->hte_index, nesqp->hte_index & adapter->hte_index_mask);
+		nesqp->hte_index, nesqp->hte_index & adapter->hte_index_mask);
 
 	nesqp->hte_index &= adapter->hte_index_mask;
 	nesqp->nesqp_context->hte_index = cpu_to_le32(nesqp->hte_index);
 
 	cm_node->cm_core->api->accelerated(cm_node->cm_core, cm_node);
 
-	nes_debug(NES_DBG_CM, "QP%u, Destination IP = 0x%08X:0x%04X, local = 0x%08X:0x%04X,"
-			" rcv_nxt=0x%08X, snd_nxt=0x%08X, mpa + private data length=%zu.\n",
-			nesqp->hwqp.qp_id,
+	nes_debug(NES_DBG_CM, "QP%u, Destination IP = 0x%08X:0x%04X, local = "
+			"0x%08X:0x%04X, rcv_nxt=0x%08X, snd_nxt=0x%08X, mpa + "
+			"private data length=%zu.\n", nesqp->hwqp.qp_id,
 			ntohl(cm_id->remote_addr.sin_addr.s_addr),
 			ntohs(cm_id->remote_addr.sin_port),
 			ntohl(cm_id->local_addr.sin_addr.s_addr),
 			ntohs(cm_id->local_addr.sin_port),
 			le32_to_cpu(nesqp->nesqp_context->rcv_nxt),
 			le32_to_cpu(nesqp->nesqp_context->snd_nxt),
-			conn_param->private_data_len+sizeof(struct ietf_mpa_frame));
+			conn_param->private_data_len +
+			sizeof(struct ietf_mpa_frame));
 
 	attr.qp_state = IB_QPS_RTS;
 	nes_modify_qp(&nesqp->ibqp, &attr, IB_QP_STATE, NULL);
@@ -2489,15 +2715,16 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	cm_event.private_data_len = 0;
 	ret = cm_id->event_handler(cm_id, &cm_event);
 	if (cm_node->loopbackpartner) {
-		cm_node->loopbackpartner->mpa_frame_size = nesqp->private_data_len;
+		cm_node->loopbackpartner->mpa_frame_size =
+			nesqp->private_data_len;
 		/* copy entire MPA frame to our cm_node's frame */
-		memcpy(cm_node->loopbackpartner->mpa_frame_buf, nesqp->ietf_frame->priv_data,
-			   nesqp->private_data_len);
+		memcpy(cm_node->loopbackpartner->mpa_frame_buf,
+			nesqp->ietf_frame->priv_data, nesqp->private_data_len);
 		create_event(cm_node->loopbackpartner, NES_CM_EVENT_CONNECTED);
 	}
 	if (ret)
-		printk("%s[%u] OFA CM event_handler returned, ret=%d\n",
-				__func__, __LINE__, ret);
+		printk(KERN_ERR "%s[%u] OFA CM event_handler returned, "
+			"ret=%d\n", __func__, __LINE__, ret);
 
 	return 0;
 }
@@ -2555,74 +2782,61 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	if (!nesdev)
 		return -EINVAL;
 
-	atomic_inc(&cm_connects);
-
-	nesqp->ietf_frame = kzalloc(sizeof(struct ietf_mpa_frame) +
-			conn_param->private_data_len, GFP_KERNEL);
-	if (!nesqp->ietf_frame)
-		return -ENOMEM;
+	nes_debug(NES_DBG_CM, "QP%u, current IP = 0x%08X, Destination IP = "
+		"0x%08X:0x%04X, local = 0x%08X:0x%04X.\n", nesqp->hwqp.qp_id,
+		ntohl(nesvnic->local_ipaddr),
+		ntohl(cm_id->remote_addr.sin_addr.s_addr),
+		ntohs(cm_id->remote_addr.sin_port),
+		ntohl(cm_id->local_addr.sin_addr.s_addr),
+		ntohs(cm_id->local_addr.sin_port));
 
-	/* set qp as having an active connection */
+	atomic_inc(&cm_connects);
 	nesqp->active_conn = 1;
 
-	nes_debug(NES_DBG_CM, "QP%u, Destination IP = 0x%08X:0x%04X, local = 0x%08X:0x%04X.\n",
-			nesqp->hwqp.qp_id,
-			ntohl(cm_id->remote_addr.sin_addr.s_addr),
-			ntohs(cm_id->remote_addr.sin_port),
-			ntohl(cm_id->local_addr.sin_addr.s_addr),
-			ntohs(cm_id->local_addr.sin_port));
-
 	/* cache the cm_id in the qp */
 	nesqp->cm_id = cm_id;
 
 	cm_id->provider_data = nesqp;
 
-	/* copy the private data */
-	if (conn_param->private_data_len) {
-		memcpy(nesqp->ietf_frame->priv_data, conn_param->private_data,
-				conn_param->private_data_len);
-	}
-
 	nesqp->private_data_len = conn_param->private_data_len;
 	nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32((u32)conn_param->ord);
 	nes_debug(NES_DBG_CM, "requested ord = 0x%08X.\n", (u32)conn_param->ord);
-	nes_debug(NES_DBG_CM, "mpa private data len =%u\n", conn_param->private_data_len);
-
-	strcpy(&nesqp->ietf_frame->key[0], IEFT_MPA_KEY_REQ);
-	nesqp->ietf_frame->flags = IETF_MPA_FLAGS_CRC;
-	nesqp->ietf_frame->rev = IETF_MPA_VERSION;
-	nesqp->ietf_frame->priv_data_len = htons(conn_param->private_data_len);
+	nes_debug(NES_DBG_CM, "mpa private data len =%u\n",
+		conn_param->private_data_len);
 
-	if (cm_id->local_addr.sin_addr.s_addr != cm_id->remote_addr.sin_addr.s_addr)
+	if (cm_id->local_addr.sin_addr.s_addr !=
+		cm_id->remote_addr.sin_addr.s_addr)
 		nes_manage_apbvt(nesvnic, ntohs(cm_id->local_addr.sin_port),
-				PCI_FUNC(nesdev->pcidev->devfn), NES_MANAGE_APBVT_ADD);
+			PCI_FUNC(nesdev->pcidev->devfn), NES_MANAGE_APBVT_ADD);
 
 	/* set up the connection params for the node */
-	cm_info.loc_addr = (cm_id->local_addr.sin_addr.s_addr);
-	cm_info.loc_port = (cm_id->local_addr.sin_port);
-	cm_info.rem_addr = (cm_id->remote_addr.sin_addr.s_addr);
-	cm_info.rem_port = (cm_id->remote_addr.sin_port);
+	cm_info.loc_addr = htonl(cm_id->local_addr.sin_addr.s_addr);
+	cm_info.loc_port = htons(cm_id->local_addr.sin_port);
+	cm_info.rem_addr = htonl(cm_id->remote_addr.sin_addr.s_addr);
+	cm_info.rem_port = htons(cm_id->remote_addr.sin_port);
 	cm_info.cm_id = cm_id;
 	cm_info.conn_type = NES_CM_IWARP_CONN_TYPE;
 
 	cm_id->add_ref(cm_id);
-	nes_add_ref(&nesqp->ibqp);
 
 	/* create a connect CM node connection */
-	cm_node = g_cm_core->api->connect(g_cm_core, nesvnic, nesqp->ietf_frame, &cm_info);
+	cm_node = g_cm_core->api->connect(g_cm_core, nesvnic,
+		conn_param->private_data_len, (void *)conn_param->private_data,
+		&cm_info);
 	if (!cm_node) {
-		if (cm_id->local_addr.sin_addr.s_addr != cm_id->remote_addr.sin_addr.s_addr)
+		if (cm_id->local_addr.sin_addr.s_addr !=
+				cm_id->remote_addr.sin_addr.s_addr)
 			nes_manage_apbvt(nesvnic, ntohs(cm_id->local_addr.sin_port),
-					PCI_FUNC(nesdev->pcidev->devfn), NES_MANAGE_APBVT_DEL);
-		nes_rem_ref(&nesqp->ibqp);
-		kfree(nesqp->ietf_frame);
-		nesqp->ietf_frame = NULL;
+				PCI_FUNC(nesdev->pcidev->devfn),
+				NES_MANAGE_APBVT_DEL);
+
 		cm_id->rem_ref(cm_id);
 		return -ENOMEM;
 	}
 
 	cm_node->apbvt_set = 1;
 	nesqp->cm_node = cm_node;
+	cm_node->nesqp = nesqp;
 
 	return 0;
 }
@@ -2664,7 +2878,7 @@ int nes_create_listen(struct iw_cm_id *cm_id, int backlog)
 
 	cm_node = g_cm_core->api->listen(g_cm_core, nesvnic, &cm_info);
 	if (!cm_node) {
-		printk("%s[%u] Error returned from listen API call\n",
+		printk(KERN_ERR "%s[%u] Error returned from listen API call\n",
 				__func__, __LINE__);
 		return -ENOMEM;
 	}
@@ -2672,10 +2886,13 @@ int nes_create_listen(struct iw_cm_id *cm_id, int backlog)
 	cm_id->provider_data = cm_node;
 
 	if (!cm_node->reused_node) {
-		err = nes_manage_apbvt(nesvnic, ntohs(cm_id->local_addr.sin_port),
-				PCI_FUNC(nesvnic->nesdev->pcidev->devfn), NES_MANAGE_APBVT_ADD);
+		err = nes_manage_apbvt(nesvnic,
+			ntohs(cm_id->local_addr.sin_port),
+			PCI_FUNC(nesvnic->nesdev->pcidev->devfn),
+			NES_MANAGE_APBVT_ADD);
 		if (err) {
-			printk("nes_manage_apbvt call returned %d.\n", err);
+			printk(KERN_ERR "nes_manage_apbvt call returned %d.\n",
+				err);
 			g_cm_core->api->stop_listener(g_cm_core, (void *)cm_node);
 			return err;
 		}
@@ -2795,53 +3012,70 @@ static void cm_event_connected(struct nes_cm_event *event)
 	nes_cm_init_tsa_conn(nesqp, cm_node);
 
 	/* set the QP tsa context */
-	nesqp->nesqp_context->tcpPorts[0] = cpu_to_le16(ntohs(cm_id->local_addr.sin_port));
-	nesqp->nesqp_context->tcpPorts[1] = cpu_to_le16(ntohs(cm_id->remote_addr.sin_port));
-	nesqp->nesqp_context->ip0 = cpu_to_le32(ntohl(cm_id->remote_addr.sin_addr.s_addr));
+	nesqp->nesqp_context->tcpPorts[0] =
+		cpu_to_le16(ntohs(cm_id->local_addr.sin_port));
+	nesqp->nesqp_context->tcpPorts[1] =
+		cpu_to_le16(ntohs(cm_id->remote_addr.sin_port));
+	if (ipv4_is_loopback(cm_id->remote_addr.sin_addr.s_addr))
+		nesqp->nesqp_context->ip0 =
+			cpu_to_le32(ntohl(nesvnic->local_ipaddr));
+	else
+		nesqp->nesqp_context->ip0 =
+			cpu_to_le32(ntohl(cm_id->remote_addr.sin_addr.s_addr));
 
 	nesqp->nesqp_context->misc2 |= cpu_to_le32(
-			(u32)PCI_FUNC(nesdev->pcidev->devfn) << NES_QPCONTEXT_MISC2_SRC_IP_SHIFT);
+			(u32)PCI_FUNC(nesdev->pcidev->devfn) <<
+			NES_QPCONTEXT_MISC2_SRC_IP_SHIFT);
 	nesqp->nesqp_context->arp_index_vlan |= cpu_to_le32(
-			nes_arp_table(nesdev, le32_to_cpu(nesqp->nesqp_context->ip0),
+			nes_arp_table(nesdev,
+			le32_to_cpu(nesqp->nesqp_context->ip0),
 			NULL, NES_ARP_RESOLVE) << 16);
 	nesqp->nesqp_context->ts_val_delta = cpu_to_le32(
 			jiffies - nes_read_indexed(nesdev, NES_IDX_TCP_NOW));
 	nesqp->nesqp_context->ird_index = cpu_to_le32(nesqp->hwqp.qp_id);
 	nesqp->nesqp_context->ird_ord_sizes |=
-			cpu_to_le32((u32)1 << NES_QPCONTEXT_ORDIRD_IWARP_MODE_SHIFT);
+			cpu_to_le32((u32)1 <<
+			NES_QPCONTEXT_ORDIRD_IWARP_MODE_SHIFT);
 
 	/* Adjust tail for not having a LSMM */
 	nesqp->hwqp.sq_tail = 1;
 
 #if defined(NES_SEND_FIRST_WRITE)
-		if (cm_node->send_write0) {
-			nes_debug(NES_DBG_CM, "Sending first write.\n");
-			wqe = &nesqp->hwqp.sq_vbase[0];
-			u64temp = (unsigned long)nesqp;
-			u64temp |= NES_SW_CONTEXT_ALIGN>>1;
-			set_wqe_64bit_value(wqe->wqe_words, NES_IWARP_SQ_WQE_COMP_CTX_LOW_IDX,
-					    u64temp);
-			wqe->wqe_words[NES_IWARP_SQ_WQE_MISC_IDX] = cpu_to_le32(NES_IWARP_SQ_OP_RDMAW);
-			wqe->wqe_words[NES_IWARP_SQ_WQE_TOTAL_PAYLOAD_IDX] = 0;
-			wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_LOW_IDX] = 0;
-			wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_HIGH_IDX] = 0;
-			wqe->wqe_words[NES_IWARP_SQ_WQE_LENGTH0_IDX] = 0;
-			wqe->wqe_words[NES_IWARP_SQ_WQE_STAG0_IDX] = 0;
-
-			/* use the reserved spot on the WQ for the extra first WQE */
-			nesqp->nesqp_context->ird_ord_sizes &= cpu_to_le32(~(NES_QPCONTEXT_ORDIRD_LSMM_PRESENT |
-					NES_QPCONTEXT_ORDIRD_WRPDU | NES_QPCONTEXT_ORDIRD_ALSMM));
-			nesqp->skip_lsmm = 1;
-			nesqp->hwqp.sq_tail = 0;
-			nes_write32(nesdev->regs + NES_WQE_ALLOC,
-					(1 << 24) | 0x00800000 | nesqp->hwqp.qp_id);
-		}
+	if (cm_node->send_write0) {
+		nes_debug(NES_DBG_CM, "Sending first write.\n");
+		wqe = &nesqp->hwqp.sq_vbase[0];
+		u64temp = (unsigned long)nesqp;
+		u64temp |= NES_SW_CONTEXT_ALIGN>>1;
+		set_wqe_64bit_value(wqe->wqe_words,
+				NES_IWARP_SQ_WQE_COMP_CTX_LOW_IDX, u64temp);
+		wqe->wqe_words[NES_IWARP_SQ_WQE_MISC_IDX] =
+			cpu_to_le32(NES_IWARP_SQ_OP_RDMAW);
+		wqe->wqe_words[NES_IWARP_SQ_WQE_TOTAL_PAYLOAD_IDX] = 0;
+		wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_LOW_IDX] = 0;
+		wqe->wqe_words[NES_IWARP_SQ_WQE_FRAG0_HIGH_IDX] = 0;
+		wqe->wqe_words[NES_IWARP_SQ_WQE_LENGTH0_IDX] = 0;
+		wqe->wqe_words[NES_IWARP_SQ_WQE_STAG0_IDX] = 0;
+
+		/* use the reserved spot on the WQ for the extra first WQE */
+		nesqp->nesqp_context->ird_ord_sizes &=
+			cpu_to_le32(~(NES_QPCONTEXT_ORDIRD_LSMM_PRESENT |
+						NES_QPCONTEXT_ORDIRD_WRPDU |
+						NES_QPCONTEXT_ORDIRD_ALSMM));
+		nesqp->skip_lsmm = 1;
+		nesqp->hwqp.sq_tail = 0;
+		nes_write32(nesdev->regs + NES_WQE_ALLOC,
+				(1 << 24) | 0x00800000 | nesqp->hwqp.qp_id);
+	}
 #endif
 
 	memset(&nes_quad, 0, sizeof(nes_quad));
 
-	nes_quad.DstIpAdrIndex = cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
-	nes_quad.SrcIpadr = cm_id->remote_addr.sin_addr.s_addr;
+	nes_quad.DstIpAdrIndex =
+		cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
+	if (ipv4_is_loopback(cm_id->remote_addr.sin_addr.s_addr))
+		nes_quad.SrcIpadr = nesvnic->local_ipaddr;
+	else
+		nes_quad.SrcIpadr = cm_id->remote_addr.sin_addr.s_addr;
 	nes_quad.TcpPorts[0] = cm_id->remote_addr.sin_port;
 	nes_quad.TcpPorts[1] = cm_id->local_addr.sin_port;
 
@@ -2858,10 +3092,6 @@ static void cm_event_connected(struct nes_cm_event *event)
 	nesqp->private_data_len = (u8) cm_node->mpa_frame_size;
 	cm_node->cm_core->api->accelerated(cm_node->cm_core, cm_node);
 
-	/* modify QP state to rts */
-	attr.qp_state = IB_QPS_RTS;
-	nes_modify_qp(&nesqp->ibqp, &attr, IB_QP_STATE, NULL);
-
 	/* notify OF layer we successfully created the requested connection */
 	cm_event.event = IW_CM_EVENT_CONNECT_REPLY;
 	cm_event.status = IW_CM_EVENT_STATUS_ACCEPTED;
@@ -2870,20 +3100,21 @@ static void cm_event_connected(struct nes_cm_event *event)
 	cm_event.local_addr.sin_port = cm_id->local_addr.sin_port;
 	cm_event.remote_addr = cm_id->remote_addr;
 
-		cm_event.private_data = (void *)event->cm_node->mpa_frame_buf;
-		cm_event.private_data_len = (u8) event->cm_node->mpa_frame_size;
+	cm_event.private_data = (void *)event->cm_node->mpa_frame_buf;
+	cm_event.private_data_len = (u8) event->cm_node->mpa_frame_size;
 
 	cm_event.local_addr.sin_addr.s_addr = event->cm_info.rem_addr;
 	ret = cm_id->event_handler(cm_id, &cm_event);
 	nes_debug(NES_DBG_CM, "OFA CM event_handler returned, ret=%d\n", ret);
 
 	if (ret)
-		printk("%s[%u] OFA CM event_handler returned, ret=%d\n",
-				__func__, __LINE__, ret);
-	nes_debug(NES_DBG_CM, "Exiting connect thread for QP%u. jiffies = %lu\n",
-			nesqp->hwqp.qp_id, jiffies );
+		printk(KERN_ERR "%s[%u] OFA CM event_handler returned, "
+			"ret=%d\n", __func__, __LINE__, ret);
+	attr.qp_state = IB_QPS_RTS;
+	nes_modify_qp(&nesqp->ibqp, &attr, IB_QP_STATE, NULL);
 
-	nes_rem_ref(&nesqp->ibqp);
+	nes_debug(NES_DBG_CM, "Exiting connect thread for QP%u. jiffies = "
+		"%lu\n", nesqp->hwqp.qp_id, jiffies);
 
 	return;
 }
@@ -2927,17 +3158,19 @@ static void cm_event_connect_error(struct nes_cm_event *event)
 	cm_event.private_data = NULL;
 	cm_event.private_data_len = 0;
 
-	nes_debug(NES_DBG_CM, "call CM_EVENT REJECTED, local_addr=%08x, remove_addr=%08x\n",
-			cm_event.local_addr.sin_addr.s_addr, cm_event.remote_addr.sin_addr.s_addr);
+	nes_debug(NES_DBG_CM, "call CM_EVENT REJECTED, local_addr=%08x, "
+		"remove_addr=%08x\n", cm_event.local_addr.sin_addr.s_addr,
+		cm_event.remote_addr.sin_addr.s_addr);
 
 	ret = cm_id->event_handler(cm_id, &cm_event);
 	nes_debug(NES_DBG_CM, "OFA CM event_handler returned, ret=%d\n", ret);
 	if (ret)
-		printk("%s[%u] OFA CM event_handler returned, ret=%d\n",
-				__func__, __LINE__, ret);
+		printk(KERN_ERR "%s[%u] OFA CM event_handler returned, "
+			"ret=%d\n", __func__, __LINE__, ret);
 	nes_rem_ref(&nesqp->ibqp);
-		cm_id->rem_ref(cm_id);
+	cm_id->rem_ref(cm_id);
 
+	rem_ref_cm_node(event->cm_node->cm_core, event->cm_node);
 	return;
 }
 
@@ -3040,7 +3273,8 @@ static int nes_cm_post_event(struct nes_cm_event *event)
 	add_ref_cm_node(event->cm_node);
 	event->cm_info.cm_id->add_ref(event->cm_info.cm_id);
 	INIT_WORK(&event->event_work, nes_cm_event_handler);
-	nes_debug(NES_DBG_CM, "queue_work, event=%p\n", event);
+	nes_debug(NES_DBG_CM, "cm_node=%p queue_work, event=%p\n",
+		event->cm_node, event);
 
 	queue_work(event->cm_node->cm_core->event_wq, &event->event_work);
 
@@ -3056,46 +3290,48 @@ static int nes_cm_post_event(struct nes_cm_event *event)
  */
 static void nes_cm_event_handler(struct work_struct *work)
 {
-	struct nes_cm_event *event = container_of(work, struct nes_cm_event, event_work);
+	struct nes_cm_event *event = container_of(work, struct nes_cm_event,
+			event_work);
 	struct nes_cm_core *cm_core;
 
-	if ((!event) || (!event->cm_node) || (!event->cm_node->cm_core)) {
+	if ((!event) || (!event->cm_node) || (!event->cm_node->cm_core))
 		return;
-	}
+
 	cm_core = event->cm_node->cm_core;
 	nes_debug(NES_DBG_CM, "event=%p, event->type=%u, events posted=%u\n",
-			event, event->type, atomic_read(&cm_core->events_posted));
+		event, event->type, atomic_read(&cm_core->events_posted));
 
 	switch (event->type) {
-		case NES_CM_EVENT_MPA_REQ:
-			cm_event_mpa_req(event);
-			nes_debug(NES_DBG_CM, "CM Event: MPA REQUEST\n");
-			break;
-		case NES_CM_EVENT_RESET:
-			nes_debug(NES_DBG_CM, "CM Event: RESET\n");
-			cm_event_reset(event);
-			break;
-		case NES_CM_EVENT_CONNECTED:
-			if ((!event->cm_node->cm_id) ||
-				(event->cm_node->state != NES_CM_STATE_TSA)) {
-				break;
-			}
-			cm_event_connected(event);
-			nes_debug(NES_DBG_CM, "CM Event: CONNECTED\n");
+	case NES_CM_EVENT_MPA_REQ:
+		cm_event_mpa_req(event);
+		nes_debug(NES_DBG_CM, "cm_node=%p CM Event: MPA REQUEST\n",
+			event->cm_node);
+		break;
+	case NES_CM_EVENT_RESET:
+		nes_debug(NES_DBG_CM, "cm_node = %p CM Event: RESET\n",
+			event->cm_node);
+		cm_event_reset(event);
+		break;
+	case NES_CM_EVENT_CONNECTED:
+		if ((!event->cm_node->cm_id) ||
+			(event->cm_node->state != NES_CM_STATE_TSA))
 			break;
-		case NES_CM_EVENT_ABORTED:
-			if ((!event->cm_node->cm_id) || (event->cm_node->state == NES_CM_STATE_TSA)) {
-				break;
-			}
-			cm_event_connect_error(event);
-			nes_debug(NES_DBG_CM, "CM Event: ABORTED\n");
-			break;
-		case NES_CM_EVENT_DROPPED_PKT:
-			nes_debug(NES_DBG_CM, "CM Event: DROPPED PKT\n");
-			break;
-		default:
-			nes_debug(NES_DBG_CM, "CM Event: UNKNOWN EVENT TYPE\n");
+		cm_event_connected(event);
+		nes_debug(NES_DBG_CM, "CM Event: CONNECTED\n");
+		break;
+	case NES_CM_EVENT_ABORTED:
+		if ((!event->cm_node->cm_id) ||
+			(event->cm_node->state == NES_CM_STATE_TSA))
 			break;
+		cm_event_connect_error(event);
+		nes_debug(NES_DBG_CM, "CM Event: ABORTED\n");
+		break;
+	case NES_CM_EVENT_DROPPED_PKT:
+		nes_debug(NES_DBG_CM, "CM Event: DROPPED PKT\n");
+		break;
+	default:
+		nes_debug(NES_DBG_CM, "CM Event: UNKNOWN EVENT TYPE\n");
+		break;
 	}
 
 	atomic_dec(&cm_core->events_posted);
diff --git a/drivers/infiniband/hw/nes/nes_cm.h b/drivers/infiniband/hw/nes/nes_cm.h
index 7717cb2ab50..367b3d29014 100644
--- a/drivers/infiniband/hw/nes/nes_cm.h
+++ b/drivers/infiniband/hw/nes/nes_cm.h
@@ -83,6 +83,8 @@ enum nes_timer_type {
 #define SET_FIN 4
 #define SET_RST 8
 
+#define TCP_OPTIONS_PADDING	3
+
 struct option_base {
 	u8 optionnum;
 	u8 length;
@@ -177,6 +179,7 @@ enum nes_cm_node_state {
 	NES_CM_STATE_ESTABLISHED,
 	NES_CM_STATE_ACCEPTING,
 	NES_CM_STATE_MPAREQ_SENT,
+	NES_CM_STATE_MPAREQ_RCVD,
 	NES_CM_STATE_TSA,
 	NES_CM_STATE_FIN_WAIT1,
 	NES_CM_STATE_FIN_WAIT2,
@@ -187,6 +190,16 @@ enum nes_cm_node_state {
 	NES_CM_STATE_CLOSED
 };
 
+enum nes_tcpip_pkt_type {
+	NES_PKT_TYPE_UNKNOWN,
+	NES_PKT_TYPE_SYN,
+	NES_PKT_TYPE_SYNACK,
+	NES_PKT_TYPE_ACK,
+	NES_PKT_TYPE_FIN,
+	NES_PKT_TYPE_RST
+};
+
+
 /* type of nes connection */
 enum nes_cm_conn_type {
 	NES_CM_IWARP_CONN_TYPE,
@@ -257,7 +270,9 @@ struct nes_cm_node {
 	struct net_device         *netdev;
 
 	struct nes_cm_node        *loopbackpartner;
-	struct list_head          retrans_list;
+
+	struct nes_timer_entry	*send_entry;
+
 	spinlock_t                retrans_list_lock;
 	struct list_head          recv_list;
 	spinlock_t                recv_list_lock;
@@ -276,6 +291,8 @@ struct nes_cm_node {
 	struct nes_vnic           *nesvnic;
 	int                       apbvt_set;
 	int                       accept_pend;
+	int			freed;
+	struct nes_qp		*nesqp;
 };
 
 /* structure for client or CM to fill when making CM api calls. */
@@ -366,14 +383,14 @@ struct nes_cm_ops {
 			struct nes_cm_info *);
 	int (*stop_listener)(struct nes_cm_core *, struct nes_cm_listener *);
 	struct nes_cm_node * (*connect)(struct nes_cm_core *,
-			struct nes_vnic *, struct ietf_mpa_frame *,
+			struct nes_vnic *, u16, void *,
 			struct nes_cm_info *);
 	int (*close)(struct nes_cm_core *, struct nes_cm_node *);
 	int (*accept)(struct nes_cm_core *, struct ietf_mpa_frame *,
 			struct nes_cm_node *);
 	int (*reject)(struct nes_cm_core *, struct ietf_mpa_frame *,
 			struct nes_cm_node *);
-	int (*recv_pkt)(struct nes_cm_core *, struct nes_vnic *,
+	void (*recv_pkt)(struct nes_cm_core *, struct nes_vnic *,
 			struct sk_buff *);
 	int (*destroy_cm_core)(struct nes_cm_core *);
 	int (*get)(struct nes_cm_core *);
diff --git a/drivers/infiniband/hw/nes/nes_hw.c b/drivers/infiniband/hw/nes/nes_hw.c
index 85f26d19a32..1513d4066f1 100644
--- a/drivers/infiniband/hw/nes/nes_hw.c
+++ b/drivers/infiniband/hw/nes/nes_hw.c
@@ -2814,7 +2814,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 			nesqp = *((struct nes_qp **)&context);
 			if (atomic_inc_return(&nesqp->close_timer_started) == 1) {
 				nesqp->cm_id->add_ref(nesqp->cm_id);
-				nes_add_ref(&nesqp->ibqp);
 				schedule_nes_timer(nesqp->cm_node, (struct sk_buff *)nesqp,
 						NES_TIMER_TYPE_CLOSE, 1, 0);
 				nes_debug(NES_DBG_AEQ, "QP%u Not decrementing QP refcount (%d),"
@@ -2838,7 +2837,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 			if (async_event_id == NES_AEQE_AEID_RESET_SENT) {
 				tcp_state = NES_AEQE_TCP_STATE_CLOSED;
 			}
-			nes_add_ref(&nesqp->ibqp);
 			spin_lock_irqsave(&nesqp->lock, flags);
 			nesqp->hw_iwarp_state = iwarp_state;
 			nesqp->hw_tcp_state = tcp_state;
@@ -2876,7 +2874,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 				}
 				spin_unlock_irqrestore(&nesqp->lock, flags);
 				if (next_iwarp_state) {
-					nes_add_ref(&nesqp->ibqp);
 					nes_debug(NES_DBG_AEQ, "issuing hw modifyqp for QP%u. next state = 0x%08X,"
 							" also added another reference\n",
 							nesqp->hwqp.qp_id, next_iwarp_state);
@@ -2888,7 +2885,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 					/* FIN Received but ib state not RTS,
 							close complete will be on its way */
 					spin_unlock_irqrestore(&nesqp->lock, flags);
-					nes_rem_ref(&nesqp->ibqp);
 					return;
 				}
 				spin_unlock_irqrestore(&nesqp->lock, flags);
@@ -2922,7 +2918,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 			if ((tcp_state == NES_AEQE_TCP_STATE_CLOSE_WAIT) ||
 					((nesqp->ibqp_state == IB_QPS_RTS)&&
 					(async_event_id == NES_AEQE_AEID_LLP_CONNECTION_RESET))) {
-				nes_add_ref(&nesqp->ibqp);
 				nes_cm_disconn(nesqp);
 			} else {
 				nesqp->in_disconnect = 0;
@@ -2931,7 +2926,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 			break;
 		case NES_AEQE_AEID_LLP_TOO_MANY_RETRIES:
 			nesqp = *((struct nes_qp **)&context);
-			nes_add_ref(&nesqp->ibqp);
 			spin_lock_irqsave(&nesqp->lock, flags);
 			nesqp->hw_iwarp_state = NES_AEQE_IWARP_STATE_ERROR;
 			nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
@@ -3042,7 +3036,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 				nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
 			}
 			/* tell cm to disconnect, cm will queue work to thread */
-			nes_add_ref(&nesqp->ibqp);
 			nes_cm_disconn(nesqp);
 			break;
 		case NES_AEQE_AEID_DDP_UBE_INVALID_MSN_NO_BUFFER_AVAILABLE:
@@ -3062,7 +3055,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 				nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
 			}
 			/* tell cm to disconnect, cm will queue work to thread */
-			nes_add_ref(&nesqp->ibqp);
 			nes_cm_disconn(nesqp);
 			break;
 		case NES_AEQE_AEID_LLP_RECEIVED_MPA_CRC_ERROR:
@@ -3082,7 +3074,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 				nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
 			}
 			/* tell cm to disconnect, cm will queue work to thread */
-			nes_add_ref(&nesqp->ibqp);
 			nes_cm_disconn(nesqp);
 			break;
 			/* TODO: additional AEs need to be here */
diff --git a/drivers/infiniband/hw/nes/nes_verbs.c b/drivers/infiniband/hw/nes/nes_verbs.c
index e3939d13484..d79942e8497 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.c
+++ b/drivers/infiniband/hw/nes/nes_verbs.c
@@ -2867,7 +2867,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 			nesqp->hwqp.qp_id, attr->qp_state, nesqp->ibqp_state,
 			nesqp->iwarp_state, atomic_read(&nesqp->refcount));
 
-	nes_add_ref(&nesqp->ibqp);
 	spin_lock_irqsave(&nesqp->lock, qplockflags);
 
 	nes_debug(NES_DBG_MOD_QP, "QP%u: hw_iwarp_state=0x%X, hw_tcp_state=0x%X,"
@@ -2882,7 +2881,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 						nesqp->hwqp.qp_id);
 				if (nesqp->iwarp_state > (u32)NES_CQP_QP_IWARP_STATE_IDLE) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				next_iwarp_state = NES_CQP_QP_IWARP_STATE_IDLE;
@@ -2893,7 +2891,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 						nesqp->hwqp.qp_id);
 				if (nesqp->iwarp_state>(u32)NES_CQP_QP_IWARP_STATE_IDLE) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				next_iwarp_state = NES_CQP_QP_IWARP_STATE_IDLE;
@@ -2904,14 +2901,12 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 						nesqp->hwqp.qp_id);
 				if (nesqp->iwarp_state>(u32)NES_CQP_QP_IWARP_STATE_RTS) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				if (nesqp->cm_id == NULL) {
 					nes_debug(NES_DBG_MOD_QP, "QP%u: Failing attempt to move QP to RTS without a CM_ID. \n",
 							nesqp->hwqp.qp_id );
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				next_iwarp_state = NES_CQP_QP_IWARP_STATE_RTS;
@@ -2929,7 +2924,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 						nesqp->hwqp.qp_id, nesqp->hwqp.sq_head, nesqp->hwqp.sq_tail);
 				if (nesqp->iwarp_state == (u32)NES_CQP_QP_IWARP_STATE_CLOSING) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return 0;
 				} else {
 					if (nesqp->iwarp_state > (u32)NES_CQP_QP_IWARP_STATE_CLOSING) {
@@ -2937,7 +2931,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 								" ignored due to current iWARP state\n",
 								nesqp->hwqp.qp_id);
 						spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-						nes_rem_ref(&nesqp->ibqp);
 						return -EINVAL;
 					}
 					if (nesqp->hw_iwarp_state != NES_AEQE_IWARP_STATE_RTS) {
@@ -2969,7 +2962,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 						nesqp->hwqp.qp_id);
 				if (nesqp->iwarp_state>=(u32)NES_CQP_QP_IWARP_STATE_TERMINATE) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				/* next_iwarp_state = (NES_CQP_QP_IWARP_STATE_TERMINATE | 0x02000000); */
@@ -2982,7 +2974,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 			case IB_QPS_RESET:
 				if (nesqp->iwarp_state == (u32)NES_CQP_QP_IWARP_STATE_ERROR) {
 					spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-					nes_rem_ref(&nesqp->ibqp);
 					return -EINVAL;
 				}
 				nes_debug(NES_DBG_MOD_QP, "QP%u: new state = error\n",
@@ -3008,7 +2999,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 				break;
 			default:
 				spin_unlock_irqrestore(&nesqp->lock, qplockflags);
-				nes_rem_ref(&nesqp->ibqp);
 				return -EINVAL;
 				break;
 		}
@@ -3088,7 +3078,6 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 							nesqp->hwqp.qp_id, atomic_read(&nesqp->refcount),
 							original_last_aeq, nesqp->last_aeq);
 					/* this one is for the cm_disconnect thread */
-					nes_add_ref(&nesqp->ibqp);
 					spin_lock_irqsave(&nesqp->lock, qplockflags);
 					nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
 					nesqp->last_aeq = NES_AEQE_AEID_RESET_SENT;
@@ -3097,14 +3086,12 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 				} else {
 					nes_debug(NES_DBG_MOD_QP, "QP%u No fake disconnect, QP refcount=%d\n",
 							nesqp->hwqp.qp_id, atomic_read(&nesqp->refcount));
-					nes_rem_ref(&nesqp->ibqp);
 				}
 			} else {
 				spin_lock_irqsave(&nesqp->lock, qplockflags);
 				if (nesqp->cm_id) {
 					/* These two are for the timer thread */
 					if (atomic_inc_return(&nesqp->close_timer_started) == 1) {
-						nes_add_ref(&nesqp->ibqp);
 						nesqp->cm_id->add_ref(nesqp->cm_id);
 						nes_debug(NES_DBG_MOD_QP, "QP%u Not decrementing QP refcount (%d),"
 								" need ae to finish up, original_last_aeq = 0x%04X."
@@ -3128,14 +3115,12 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 					" original_last_aeq = 0x%04X. last_aeq = 0x%04X.\n",
 					nesqp->hwqp.qp_id, atomic_read(&nesqp->refcount),
 					original_last_aeq, nesqp->last_aeq);
-			nes_rem_ref(&nesqp->ibqp);
 		}
 	} else {
 		nes_debug(NES_DBG_MOD_QP, "QP%u Decrementing QP refcount (%d), No ae to finish up,"
 				" original_last_aeq = 0x%04X. last_aeq = 0x%04X.\n",
 				nesqp->hwqp.qp_id, atomic_read(&nesqp->refcount),
 				original_last_aeq, nesqp->last_aeq);
-		nes_rem_ref(&nesqp->ibqp);
 	}
 
 	err = 0;
diff --git a/drivers/infiniband/ulp/ipoib/Kconfig b/drivers/infiniband/ulp/ipoib/Kconfig
index 691525cf394..9d9a9dc51f1 100644
--- a/drivers/infiniband/ulp/ipoib/Kconfig
+++ b/drivers/infiniband/ulp/ipoib/Kconfig
@@ -11,16 +11,17 @@ config INFINIBAND_IPOIB
 
 config INFINIBAND_IPOIB_CM
 	bool "IP-over-InfiniBand Connected Mode support"
-	depends on INFINIBAND_IPOIB && EXPERIMENTAL
+	depends on INFINIBAND_IPOIB
 	default n
 	---help---
-	  This option enables experimental support for IPoIB connected mode.
-	  After enabling this option, you need to switch to connected mode through
-	  /sys/class/net/ibXXX/mode to actually create connections, and then increase
-	  the interface MTU with e.g. ifconfig ib0 mtu 65520.
+	  This option enables support for IPoIB connected mode.  After
+	  enabling this option, you need to switch to connected mode
+	  through /sys/class/net/ibXXX/mode to actually create
+	  connections, and then increase the interface MTU with
+	  e.g. ifconfig ib0 mtu 65520.
 
-	  WARNING: Enabling connected mode will trigger some
-	  packet drops for multicast and UD mode traffic from this interface,
+	  WARNING: Enabling connected mode will trigger some packet
+	  drops for multicast and UD mode traffic from this interface,
 	  unless you limit mtu for these destinations to 2044.
 
 config INFINIBAND_IPOIB_DEBUG
@@ -33,9 +34,10 @@ config INFINIBAND_IPOIB_DEBUG
 	  debug_level and mcast_debug_level module parameters (which
 	  can also be set after the driver is loaded through sysfs).
 
-	  This option also creates an "ipoib_debugfs," which can be
-	  mounted to expose debugging information about IB multicast
-	  groups used by the IPoIB driver.
+	  This option also creates a directory tree under ipoib/ in
+	  debugfs, which contains files that expose debugging
+	  information about IB multicast groups used by the IPoIB
+	  driver.
 
 config INFINIBAND_IPOIB_DEBUG_DATA
 	bool "IP-over-InfiniBand data path debugging"
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 8be9ea0436e..f51201b17bf 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -548,7 +548,7 @@ static int path_rec_start(struct net_device *dev,
 				   path_rec_completion,
 				   path, &path->query);
 	if (path->query_id < 0) {
-		ipoib_warn(priv, "ib_sa_path_rec_get failed\n");
+		ipoib_warn(priv, "ib_sa_path_rec_get failed: %d\n", path->query_id);
 		path->query = NULL;
 		return path->query_id;
 	}
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 3a917c1f796..63462ecca14 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -483,6 +483,7 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 		break;
 	case RDMA_CM_EVENT_DISCONNECTED:
 	case RDMA_CM_EVENT_DEVICE_REMOVAL:
+	case RDMA_CM_EVENT_ADDR_CHANGE:
 		iser_disconnected_handler(cma_id);
 		break;
 	default:
diff --git a/drivers/input/keyboard/hil_kbd.c b/drivers/input/keyboard/hil_kbd.c
index adbf29f0169..71c1971abf8 100644
--- a/drivers/input/keyboard/hil_kbd.c
+++ b/drivers/input/keyboard/hil_kbd.c
@@ -37,6 +37,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
+#include <linux/semaphore.h>
 #include <linux/slab.h>
 #include <linux/pci_ids.h>
 
diff --git a/drivers/input/misc/hp_sdc_rtc.c b/drivers/input/misc/hp_sdc_rtc.c
index 49d8abfe38f..daa9d422033 100644
--- a/drivers/input/misc/hp_sdc_rtc.c
+++ b/drivers/input/misc/hp_sdc_rtc.c
@@ -44,6 +44,7 @@
 #include <linux/proc_fs.h>
 #include <linux/poll.h>
 #include <linux/rtc.h>
+#include <linux/semaphore.h>
 
 MODULE_AUTHOR("Brian S. Julin <bri@calyx.com>");
 MODULE_DESCRIPTION("HP i8042 SDC + MSM-58321 RTC Driver");
diff --git a/drivers/input/serio/hp_sdc.c b/drivers/input/serio/hp_sdc.c
index 7b233a492ad..0d395979b2d 100644
--- a/drivers/input/serio/hp_sdc.c
+++ b/drivers/input/serio/hp_sdc.c
@@ -67,9 +67,9 @@
 #include <linux/module.h>
 #include <linux/ioport.h>
 #include <linux/time.h>
+#include <linux/semaphore.h>
 #include <linux/slab.h>
 #include <linux/hil.h>
-#include <linux/semaphore.h>
 #include <asm/io.h>
 #include <asm/system.h>
 
diff --git a/drivers/isdn/Kconfig b/drivers/isdn/Kconfig
index 66f946aa30b..3d113c6e4a7 100644
--- a/drivers/isdn/Kconfig
+++ b/drivers/isdn/Kconfig
@@ -3,7 +3,7 @@
 #
 
 menuconfig ISDN
-	tristate "ISDN support"
+	bool "ISDN support"
 	depends on NET
 	depends on !S390
 	---help---
@@ -21,6 +21,8 @@ menuconfig ISDN
 
 if ISDN
 
+source "drivers/isdn/mISDN/Kconfig"
+
 menuconfig ISDN_I4L
 	tristate "Old ISDN4Linux (deprecated)"
 	---help---
diff --git a/drivers/isdn/Makefile b/drivers/isdn/Makefile
index 988142c30a6..8380a4568d1 100644
--- a/drivers/isdn/Makefile
+++ b/drivers/isdn/Makefile
@@ -4,6 +4,7 @@
 
 obj-$(CONFIG_ISDN_I4L)			+= i4l/
 obj-$(CONFIG_ISDN_CAPI)			+= capi/
+obj-$(CONFIG_MISDN)			+= mISDN/
 obj-$(CONFIG_ISDN_CAPI)			+= hardware/
 obj-$(CONFIG_ISDN_DIVERSION)		+= divert/
 obj-$(CONFIG_ISDN_DRV_HISAX)		+= hisax/
diff --git a/drivers/isdn/gigaset/asyncdata.c b/drivers/isdn/gigaset/asyncdata.c
index 091deb9d1c4..c2bd97d2927 100644
--- a/drivers/isdn/gigaset/asyncdata.c
+++ b/drivers/isdn/gigaset/asyncdata.c
@@ -575,7 +575,8 @@ int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
 	else
 		skb = iraw_encode(skb, HW_HDR_LEN, 0);
 	if (!skb) {
-		err("unable to allocate memory for encoding!\n");
+		dev_err(bcs->cs->dev,
+			"unable to allocate memory for encoding!\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/isdn/gigaset/bas-gigaset.c b/drivers/isdn/gigaset/bas-gigaset.c
index 5255b5e20e1..3f11910c7cc 100644
--- a/drivers/isdn/gigaset/bas-gigaset.c
+++ b/drivers/isdn/gigaset/bas-gigaset.c
@@ -1050,10 +1050,9 @@ static int submit_iso_write_urb(struct isow_urbctx_t *ucx)
 		}
 
 		/* retrieve block of data to send */
-		ifd->offset = gigaset_isowbuf_getbytes(ubc->isooutbuf,
-						       ifd->length);
-		if (ifd->offset < 0) {
-			if (ifd->offset == -EBUSY) {
+		rc = gigaset_isowbuf_getbytes(ubc->isooutbuf, ifd->length);
+		if (rc < 0) {
+			if (rc == -EBUSY) {
 				gig_dbg(DEBUG_ISO,
 					"%s: buffer busy at frame %d",
 					__func__, nframe);
@@ -1062,11 +1061,12 @@ static int submit_iso_write_urb(struct isow_urbctx_t *ucx)
 			} else {
 				dev_err(ucx->bcs->cs->dev,
 					"%s: buffer error %d at frame %d\n",
-					__func__, ifd->offset, nframe);
-				return ifd->offset;
+					__func__, rc, nframe);
+				return rc;
 			}
 			break;
 		}
+		ifd->offset = rc;
 		ucx->limit = ubc->isooutbuf->nextread;
 		ifd->status = 0;
 		ifd->actual_length = 0;
diff --git a/drivers/isdn/gigaset/common.c b/drivers/isdn/gigaset/common.c
index 827c32c1679..9d3ce7718e5 100644
--- a/drivers/isdn/gigaset/common.c
+++ b/drivers/isdn/gigaset/common.c
@@ -287,7 +287,7 @@ struct event_t *gigaset_add_event(struct cardstate *cs,
 	tail = cs->ev_tail;
 	next = (tail + 1) % MAX_EVENTS;
 	if (unlikely(next == cs->ev_head))
-		err("event queue full");
+		dev_err(cs->dev, "event queue full\n");
 	else {
 		event = cs->events + tail;
 		event->type = type;
diff --git a/drivers/isdn/gigaset/gigaset.h b/drivers/isdn/gigaset/gigaset.h
index f365993161f..00375295499 100644
--- a/drivers/isdn/gigaset/gigaset.h
+++ b/drivers/isdn/gigaset/gigaset.h
@@ -106,7 +106,6 @@ enum debuglevel {
 #undef err
 #undef info
 #undef warn
-#undef notice
 
 #define err(format, arg...) printk(KERN_ERR KBUILD_MODNAME ": " \
 	format "\n" , ## arg)
@@ -114,8 +113,6 @@ enum debuglevel {
 	format "\n" , ## arg)
 #define warn(format, arg...) printk(KERN_WARNING KBUILD_MODNAME ": " \
 	format "\n" , ## arg)
-#define notice(format, arg...) printk(KERN_NOTICE KBUILD_MODNAME ": " \
-	format "\n" , ## arg)
 
 #ifdef CONFIG_GIGASET_DEBUG
 
diff --git a/drivers/isdn/gigaset/i4l.c b/drivers/isdn/gigaset/i4l.c
index 9e089f06a94..3c127a8cbaf 100644
--- a/drivers/isdn/gigaset/i4l.c
+++ b/drivers/isdn/gigaset/i4l.c
@@ -46,7 +46,8 @@ static int writebuf_from_LL(int driverID, int channel, int ack,
 		return -ENODEV;
 	}
 	if (channel < 0 || channel >= cs->channels) {
-		err("%s: invalid channel ID (%d)", __func__, channel);
+		dev_err(cs->dev, "%s: invalid channel ID (%d)\n",
+			__func__, channel);
 		return -ENODEV;
 	}
 	bcs = &cs->bcs[channel];
@@ -58,11 +59,13 @@ static int writebuf_from_LL(int driverID, int channel, int ack,
 
 	if (!len) {
 		if (ack)
-			notice("%s: not ACKing empty packet", __func__);
+			dev_notice(cs->dev, "%s: not ACKing empty packet\n",
+				   __func__);
 		return 0;
 	}
 	if (len > MAX_BUF_SIZE) {
-		err("%s: packet too large (%d bytes)", __func__, len);
+		dev_err(cs->dev, "%s: packet too large (%d bytes)\n",
+			__func__, len);
 		return -EINVAL;
 	}
 
@@ -116,8 +119,7 @@ static int command_from_LL(isdn_ctrl *cntrl)
 	gigaset_debugdrivers();
 
 	if (!cs) {
-		warn("LL tried to access unknown device with nr. %d",
-		     cntrl->driver);
+		err("%s: invalid driver ID (%d)", __func__, cntrl->driver);
 		return -ENODEV;
 	}
 
@@ -126,7 +128,7 @@ static int command_from_LL(isdn_ctrl *cntrl)
 		gig_dbg(DEBUG_ANY, "ISDN_CMD_IOCTL (driver: %d, arg: %ld)",
 			cntrl->driver, cntrl->arg);
 
-		warn("ISDN_CMD_IOCTL is not supported.");
+		dev_warn(cs->dev, "ISDN_CMD_IOCTL not supported\n");
 		return -EINVAL;
 
 	case ISDN_CMD_DIAL:
@@ -138,22 +140,23 @@ static int command_from_LL(isdn_ctrl *cntrl)
 			cntrl->parm.setup.si1, cntrl->parm.setup.si2);
 
 		if (cntrl->arg >= cs->channels) {
-			err("ISDN_CMD_DIAL: invalid channel (%d)",
-			    (int) cntrl->arg);
+			dev_err(cs->dev,
+				"ISDN_CMD_DIAL: invalid channel (%d)\n",
+				(int) cntrl->arg);
 			return -EINVAL;
 		}
 
 		bcs = cs->bcs + cntrl->arg;
 
 		if (!gigaset_get_channel(bcs)) {
-			err("ISDN_CMD_DIAL: channel not free");
+			dev_err(cs->dev, "ISDN_CMD_DIAL: channel not free\n");
 			return -EBUSY;
 		}
 
 		sp = kmalloc(sizeof *sp, GFP_ATOMIC);
 		if (!sp) {
 			gigaset_free_channel(bcs);
-			err("ISDN_CMD_DIAL: out of memory");
+			dev_err(cs->dev, "ISDN_CMD_DIAL: out of memory\n");
 			return -ENOMEM;
 		}
 		*sp = cntrl->parm.setup;
@@ -173,8 +176,9 @@ static int command_from_LL(isdn_ctrl *cntrl)
 		gig_dbg(DEBUG_ANY, "ISDN_CMD_ACCEPTD");
 
 		if (cntrl->arg >= cs->channels) {
-			err("ISDN_CMD_ACCEPTD: invalid channel (%d)",
-			    (int) cntrl->arg);
+			dev_err(cs->dev,
+				"ISDN_CMD_ACCEPTD: invalid channel (%d)\n",
+				(int) cntrl->arg);
 			return -EINVAL;
 		}
 
@@ -196,8 +200,9 @@ static int command_from_LL(isdn_ctrl *cntrl)
 			(int) cntrl->arg);
 
 		if (cntrl->arg >= cs->channels) {
-			err("ISDN_CMD_HANGUP: invalid channel (%u)",
-			    (unsigned) cntrl->arg);
+			dev_err(cs->dev,
+				"ISDN_CMD_HANGUP: invalid channel (%d)\n",
+				(int) cntrl->arg);
 			return -EINVAL;
 		}
 
@@ -224,8 +229,9 @@ static int command_from_LL(isdn_ctrl *cntrl)
 			cntrl->arg & 0xff, (cntrl->arg >> 8));
 
 		if ((cntrl->arg & 0xff) >= cs->channels) {
-			err("ISDN_CMD_SETL2: invalid channel (%u)",
-			    (unsigned) cntrl->arg & 0xff);
+			dev_err(cs->dev,
+				"ISDN_CMD_SETL2: invalid channel (%d)\n",
+				(int) cntrl->arg & 0xff);
 			return -EINVAL;
 		}
 
@@ -244,14 +250,16 @@ static int command_from_LL(isdn_ctrl *cntrl)
 			cntrl->arg & 0xff, (cntrl->arg >> 8));
 
 		if ((cntrl->arg & 0xff) >= cs->channels) {
-			err("ISDN_CMD_SETL3: invalid channel (%u)",
-			    (unsigned) cntrl->arg & 0xff);
+			dev_err(cs->dev,
+				"ISDN_CMD_SETL3: invalid channel (%d)\n",
+				(int) cntrl->arg & 0xff);
 			return -EINVAL;
 		}
 
 		if (cntrl->arg >> 8 != ISDN_PROTO_L3_TRANS) {
-			err("ISDN_CMD_SETL3: invalid protocol %lu",
-			    cntrl->arg >> 8);
+			dev_err(cs->dev,
+				"ISDN_CMD_SETL3: invalid protocol %lu\n",
+				cntrl->arg >> 8);
 			return -EINVAL;
 		}
 
@@ -262,8 +270,9 @@ static int command_from_LL(isdn_ctrl *cntrl)
 	case ISDN_CMD_ALERT:
 		gig_dbg(DEBUG_ANY, "ISDN_CMD_ALERT"); //FIXME
 		if (cntrl->arg >= cs->channels) {
-			err("ISDN_CMD_ALERT: invalid channel (%d)",
-			    (int) cntrl->arg);
+			dev_err(cs->dev,
+				"ISDN_CMD_ALERT: invalid channel (%d)\n",
+				(int) cntrl->arg);
 			return -EINVAL;
 		}
 		//bcs = cs->bcs + cntrl->arg;
@@ -295,7 +304,8 @@ static int command_from_LL(isdn_ctrl *cntrl)
 		gig_dbg(DEBUG_ANY, "ISDN_CMD_GETSIL");
 		break;
 	default:
-		err("unknown command %d from LL", cntrl->command);
+		dev_err(cs->dev, "unknown command %d from LL\n",
+			cntrl->command);
 		return -EINVAL;
 	}
 
diff --git a/drivers/isdn/gigaset/interface.c b/drivers/isdn/gigaset/interface.c
index af195b07c19..521951a898e 100644
--- a/drivers/isdn/gigaset/interface.c
+++ b/drivers/isdn/gigaset/interface.c
@@ -197,7 +197,7 @@ static void if_close(struct tty_struct *tty, struct file *filp)
 	mutex_lock(&cs->mutex);
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else {
 		if (!--cs->open_count) {
 			spin_lock_irqsave(&cs->lock, flags);
@@ -232,7 +232,7 @@ static int if_ioctl(struct tty_struct *tty, struct file *file,
 		return -ERESTARTSYS; // FIXME -EINTR?
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else {
 		retval = 0;
 		switch (cmd) {
@@ -364,9 +364,9 @@ static int if_write(struct tty_struct *tty, const unsigned char *buf, int count)
 		return -ERESTARTSYS; // FIXME -EINTR?
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else if (cs->mstate != MS_LOCKED) {
-		warn("can't write to unlocked device");
+		dev_warn(cs->dev, "can't write to unlocked device\n");
 		retval = -EBUSY;
 	} else if (!cs->connected) {
 		gig_dbg(DEBUG_ANY, "can't write to unplugged device");
@@ -398,9 +398,9 @@ static int if_write_room(struct tty_struct *tty)
 		return -ERESTARTSYS; // FIXME -EINTR?
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else if (cs->mstate != MS_LOCKED) {
-		warn("can't write to unlocked device");
+		dev_warn(cs->dev, "can't write to unlocked device\n");
 		retval = -EBUSY;
 	} else if (!cs->connected) {
 		gig_dbg(DEBUG_ANY, "can't write to unplugged device");
@@ -430,9 +430,9 @@ static int if_chars_in_buffer(struct tty_struct *tty)
 		return -ERESTARTSYS; // FIXME -EINTR?
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else if (cs->mstate != MS_LOCKED) {
-		warn("can't write to unlocked device");
+		dev_warn(cs->dev, "can't write to unlocked device\n");
 		retval = -EBUSY;
 	} else if (!cs->connected) {
 		gig_dbg(DEBUG_ANY, "can't write to unplugged device");
@@ -460,7 +460,7 @@ static void if_throttle(struct tty_struct *tty)
 	mutex_lock(&cs->mutex);
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else {
 		//FIXME
 	}
@@ -483,7 +483,7 @@ static void if_unthrottle(struct tty_struct *tty)
 	mutex_lock(&cs->mutex);
 
 	if (!cs->open_count)
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 	else {
 		//FIXME
 	}
@@ -510,7 +510,7 @@ static void if_set_termios(struct tty_struct *tty, struct ktermios *old)
 	mutex_lock(&cs->mutex);
 
 	if (!cs->open_count) {
-		warn("%s: device not opened", __func__);
+		dev_warn(cs->dev, "%s: device not opened\n", __func__);
 		goto out;
 	}
 
@@ -623,7 +623,8 @@ void gigaset_if_init(struct cardstate *cs)
 	if (!IS_ERR(cs->tty_dev))
 		dev_set_drvdata(cs->tty_dev, cs);
 	else {
-		warn("could not register device to the tty subsystem");
+		dev_warn(cs->dev,
+			 "could not register device to the tty subsystem\n");
 		cs->tty_dev = NULL;
 	}
 	mutex_unlock(&cs->mutex);
diff --git a/drivers/isdn/gigaset/usb-gigaset.c b/drivers/isdn/gigaset/usb-gigaset.c
index 77d20ab0cd4..4661830a49d 100644
--- a/drivers/isdn/gigaset/usb-gigaset.c
+++ b/drivers/isdn/gigaset/usb-gigaset.c
@@ -498,8 +498,9 @@ static int send_cb(struct cardstate *cs, struct cmdbuf_t *cb)
 
 			if (status) {
 				ucs->busy = 0;
-				err("could not submit urb (error %d)\n",
-				    -status);
+				dev_err(cs->dev,
+					"could not submit urb (error %d)\n",
+					-status);
 				cb->len = 0; /* skip urb => remove cb+wakeup
 						in next loop cycle */
 			}
@@ -670,7 +671,7 @@ static int write_modem(struct cardstate *cs)
 	spin_unlock_irqrestore(&cs->lock, flags);
 
 	if (ret) {
-		err("could not submit urb (error %d)\n", -ret);
+		dev_err(cs->dev, "could not submit urb (error %d)\n", -ret);
 		ucs->busy = 0;
 	}
 
diff --git a/drivers/isdn/hardware/Makefile b/drivers/isdn/hardware/Makefile
index 11c8a183948..a5d8fce4c4c 100644
--- a/drivers/isdn/hardware/Makefile
+++ b/drivers/isdn/hardware/Makefile
@@ -4,3 +4,4 @@
 
 obj-$(CONFIG_CAPI_AVM)		+= avm/
 obj-$(CONFIG_CAPI_EICON)	+= eicon/
+obj-$(CONFIG_MISDN)		+= mISDN/
diff --git a/drivers/isdn/hardware/mISDN/Kconfig b/drivers/isdn/hardware/mISDN/Kconfig
new file mode 100644
index 00000000000..14793480c45
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/Kconfig
@@ -0,0 +1,25 @@
+#
+# Hardware for mISDN
+#
+comment "mISDN hardware drivers"
+
+config MISDN_HFCPCI
+	tristate "Support for HFC PCI cards"
+	depends on MISDN
+	depends on PCI
+	help
+	  Enable support for cards with Cologne Chip AG's
+          HFC PCI chip.
+
+config MISDN_HFCMULTI
+	tristate "Support for HFC multiport cards (HFC-4S/8S/E1)"
+	depends on PCI
+	depends on MISDN
+	help
+	  Enable support for cards with Cologne Chip AG's HFC multiport
+	  chip. There are three types of chips that are quite similar,
+	  but the interface is different:
+	   * HFC-4S (4 S/T interfaces on one chip)
+	   * HFC-8S (8 S/T interfaces on one chip)
+	   * HFC-E1 (E1 interface for 2Mbit ISDN)
+
diff --git a/drivers/isdn/hardware/mISDN/Makefile b/drivers/isdn/hardware/mISDN/Makefile
new file mode 100644
index 00000000000..1e7ca5332ad
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the modular ISDN hardware drivers
+#
+#
+
+obj-$(CONFIG_MISDN_HFCPCI) += hfcpci.o
+obj-$(CONFIG_MISDN_HFCMULTI) += hfcmulti.o
diff --git a/drivers/isdn/hardware/mISDN/hfc_multi.h b/drivers/isdn/hardware/mISDN/hfc_multi.h
new file mode 100644
index 00000000000..a33d87afc84
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/hfc_multi.h
@@ -0,0 +1,1204 @@
+/*
+ * see notice in hfc_multi.c
+ */
+
+extern void ztdummy_extern_interrupt(void);
+extern void ztdummy_register_interrupt(void);
+extern int ztdummy_unregister_interrupt(void);
+
+#define DEBUG_HFCMULTI_FIFO	0x00010000
+#define	DEBUG_HFCMULTI_CRC	0x00020000
+#define	DEBUG_HFCMULTI_INIT	0x00040000
+#define	DEBUG_HFCMULTI_PLXSD	0x00080000
+#define	DEBUG_HFCMULTI_MODE	0x00100000
+#define	DEBUG_HFCMULTI_MSG	0x00200000
+#define	DEBUG_HFCMULTI_STATE	0x00400000
+#define	DEBUG_HFCMULTI_SYNC	0x01000000
+#define	DEBUG_HFCMULTI_DTMF	0x02000000
+#define	DEBUG_HFCMULTI_LOCK	0x80000000
+
+#define	PCI_ENA_REGIO	0x01
+#define	PCI_ENA_MEMIO	0x02
+
+/*
+ * NOTE: some registers are assigned multiple times due to different modes
+ *       also registers are assigned differen for HFC-4s/8s and HFC-E1
+ */
+
+/*
+#define MAX_FRAME_SIZE	2048
+*/
+
+struct hfc_chan {
+	struct dchannel	*dch;	/* link if channel is a D-channel */
+	struct bchannel	*bch;	/* link if channel is a B-channel */
+	int		port; 	/* the interface port this */
+				/* channel is associated with */
+	int		nt_timer; /* -1 if off, 0 if elapsed, >0 if running */
+	int		los, ais, slip_tx, slip_rx, rdi; /* current alarms */
+	int		jitter;
+	u_long		cfg;	/* port configuration */
+	int		sync;	/* sync state (used by E1) */
+	u_int		protocol; /* current protocol */
+	int		slot_tx; /* current pcm slot */
+	int		bank_tx; /* current pcm bank */
+	int		slot_rx;
+	int		bank_rx;
+	int		conf;	/* conference setting of TX slot */
+	int		txpending;	/* if there is currently data in */
+					/* the FIFO 0=no, 1=yes, 2=splloop */
+	int		rx_off; /* set to turn fifo receive off */
+	int		coeff_count; /* curren coeff block */
+	s32		*coeff; /* memory pointer to 8 coeff blocks */
+};
+
+
+struct hfcm_hw {
+	u_char	r_ctrl;
+	u_char	r_irq_ctrl;
+	u_char	r_cirm;
+	u_char	r_ram_sz;
+	u_char	r_pcm_md0;
+	u_char	r_irqmsk_misc;
+	u_char	r_dtmf;
+	u_char	r_st_sync;
+	u_char	r_sci_msk;
+	u_char	r_tx0, r_tx1;
+	u_char	a_st_ctrl0[8];
+	timer_t	timer;
+};
+
+
+/* for each stack these flags are used (cfg) */
+#define	HFC_CFG_NONCAP_TX	1 /* S/T TX interface has less capacity */
+#define	HFC_CFG_DIS_ECHANNEL	2 /* disable E-channel processing */
+#define	HFC_CFG_REG_ECHANNEL	3 /* register E-channel */
+#define	HFC_CFG_OPTICAL		4 /* the E1 interface is optical */
+#define	HFC_CFG_REPORT_LOS	5 /* the card should report loss of signal */
+#define	HFC_CFG_REPORT_AIS	6 /* the card should report alarm ind. sign. */
+#define	HFC_CFG_REPORT_SLIP	7 /* the card should report bit slips */
+#define	HFC_CFG_REPORT_RDI	8 /* the card should report remote alarm */
+#define	HFC_CFG_DTMF		9 /* enable DTMF-detection */
+#define	HFC_CFG_CRC4		10 /* disable CRC-4 Multiframe mode, */
+					/* use double frame instead. */
+
+#define	HFC_CHIP_EXRAM_128	0 /* external ram 128k */
+#define	HFC_CHIP_EXRAM_512	1 /* external ram 256k */
+#define	HFC_CHIP_REVISION0	2 /* old fifo handling */
+#define	HFC_CHIP_PCM_SLAVE	3 /* PCM is slave */
+#define	HFC_CHIP_PCM_MASTER	4 /* PCM is master */
+#define	HFC_CHIP_RX_SYNC	5 /* disable pll sync for pcm */
+#define	HFC_CHIP_DTMF		6 /* DTMF decoding is enabled */
+#define	HFC_CHIP_ULAW		7 /* ULAW mode */
+#define	HFC_CHIP_CLOCK2		8 /* double clock mode */
+#define	HFC_CHIP_E1CLOCK_GET	9 /* always get clock from E1 interface */
+#define	HFC_CHIP_E1CLOCK_PUT	10 /* always put clock from E1 interface */
+#define	HFC_CHIP_WATCHDOG	11 /* whether we should send signals */
+					/* to the watchdog */
+#define	HFC_CHIP_B410P		12 /* whether we have a b410p with echocan in */
+					/* hw */
+#define	HFC_CHIP_PLXSD		13 /* whether we have a Speech-Design PLX */
+
+#define HFC_IO_MODE_PCIMEM	0x00 /* normal memory mapped IO */
+#define HFC_IO_MODE_REGIO	0x01 /* PCI io access */
+#define HFC_IO_MODE_PLXSD	0x02 /* access HFC via PLX9030 */
+
+/* table entry in the PCI devices list */
+struct hm_map {
+	char *vendor_name;
+	char *card_name;
+	int type;
+	int ports;
+	int clock2;
+	int leds;
+	int opticalsupport;
+	int dip_type;
+	int io_mode;
+};
+
+struct hfc_multi {
+	struct list_head	list;
+	struct hm_map	*mtyp;
+	int		id;
+	int		pcm;	/* id of pcm bus */
+	int		type;
+	int		ports;
+
+	u_int		irq;	/* irq used by card */
+	u_int		irqcnt;
+	struct pci_dev	*pci_dev;
+	int		io_mode; /* selects mode */
+#ifdef HFC_REGISTER_DEBUG
+	void		(*HFC_outb)(struct hfc_multi *hc, u_char reg,
+				u_char val, const char *function, int line);
+	void		(*HFC_outb_nodebug)(struct hfc_multi *hc, u_char reg,
+				u_char val, const char *function, int line);
+	u_char		(*HFC_inb)(struct hfc_multi *hc, u_char reg,
+				const char *function, int line);
+	u_char		(*HFC_inb_nodebug)(struct hfc_multi *hc, u_char reg,
+				const char *function, int line);
+	u_short		(*HFC_inw)(struct hfc_multi *hc, u_char reg,
+				const char *function, int line);
+	u_short		(*HFC_inw_nodebug)(struct hfc_multi *hc, u_char reg,
+				const char *function, int line);
+	void		(*HFC_wait)(struct hfc_multi *hc,
+				const char *function, int line);
+	void		(*HFC_wait_nodebug)(struct hfc_multi *hc,
+				const char *function, int line);
+#else
+	void		(*HFC_outb)(struct hfc_multi *hc, u_char reg,
+				u_char val);
+	void		(*HFC_outb_nodebug)(struct hfc_multi *hc, u_char reg,
+				u_char val);
+	u_char		(*HFC_inb)(struct hfc_multi *hc, u_char reg);
+	u_char		(*HFC_inb_nodebug)(struct hfc_multi *hc, u_char reg);
+	u_short		(*HFC_inw)(struct hfc_multi *hc, u_char reg);
+	u_short		(*HFC_inw_nodebug)(struct hfc_multi *hc, u_char reg);
+	void		(*HFC_wait)(struct hfc_multi *hc);
+	void		(*HFC_wait_nodebug)(struct hfc_multi *hc);
+#endif
+	void		(*read_fifo)(struct hfc_multi *hc, u_char *data,
+				int len);
+	void		(*write_fifo)(struct hfc_multi *hc, u_char *data,
+				int len);
+	u_long		pci_origmembase, plx_origmembase, dsp_origmembase;
+	u_char		*pci_membase; /* PCI memory (MUST BE BYTE POINTER) */
+	u_char		*plx_membase; /* PLX memory */
+	u_char		*dsp_membase; /* DSP on PLX */
+	u_long		pci_iobase; /* PCI IO */
+	struct hfcm_hw	hw;	/* remember data of write-only-registers */
+
+	u_long		chip;	/* chip configuration */
+	int		masterclk; /* port that provides master clock -1=off */
+	int		dtmf;	/* flag that dtmf is currently in process */
+	int		Flen;	/* F-buffer size */
+	int		Zlen;	/* Z-buffer size (must be int for calculation)*/
+	int		max_trans; /* maximum transparent fifo fill */
+	int		Zmin;	/* Z-buffer offset */
+	int		DTMFbase; /* base address of DTMF coefficients */
+
+	u_int		slots;	/* number of PCM slots */
+	u_int		leds;	/* type of leds */
+	u_int		ledcount; /* used to animate leds */
+	u_long		ledstate; /* save last state of leds */
+	int		opticalsupport; /* has the e1 board */
+					/* an optical Interface */
+	int		dslot;	/* channel # of d-channel (E1) default 16 */
+
+	u_long		wdcount; 	/* every 500 ms we need to */
+					/* send the watchdog a signal */
+	u_char		wdbyte; /* watchdog toggle byte */
+	u_int		activity[8]; 	/* if there is any action on this */
+					/* port (will be cleared after */
+					/* showing led-states) */
+	int		e1_state; /* keep track of last state */
+	int		e1_getclock; /* if sync is retrieved from interface */
+	int		syncronized; /* keep track of existing sync interface */
+	int		e1_resync; /* resync jobs */
+
+	spinlock_t	lock;	/* the lock */
+
+	/*
+	 * the channel index is counted from 0, regardless where the channel
+	 * is located on the hfc-channel.
+	 * the bch->channel is equvalent to the hfc-channel
+	 */
+	struct hfc_chan	chan[32];
+	u_char		created[8]; /* what port is created */
+	signed char	slot_owner[256]; /* owner channel of slot */
+};
+
+/* PLX GPIOs */
+#define	PLX_GPIO4_DIR_BIT	13
+#define	PLX_GPIO4_BIT		14
+#define	PLX_GPIO5_DIR_BIT	16
+#define	PLX_GPIO5_BIT		17
+#define	PLX_GPIO6_DIR_BIT	19
+#define	PLX_GPIO6_BIT		20
+#define	PLX_GPIO7_DIR_BIT	22
+#define	PLX_GPIO7_BIT		23
+#define PLX_GPIO8_DIR_BIT	25
+#define PLX_GPIO8_BIT		26
+
+#define	PLX_GPIO4		(1 << PLX_GPIO4_BIT)
+#define	PLX_GPIO5		(1 << PLX_GPIO5_BIT)
+#define	PLX_GPIO6		(1 << PLX_GPIO6_BIT)
+#define	PLX_GPIO7		(1 << PLX_GPIO7_BIT)
+#define PLX_GPIO8		(1 << PLX_GPIO8_BIT)
+
+#define	PLX_GPIO4_DIR		(1 << PLX_GPIO4_DIR_BIT)
+#define	PLX_GPIO5_DIR		(1 << PLX_GPIO5_DIR_BIT)
+#define	PLX_GPIO6_DIR		(1 << PLX_GPIO6_DIR_BIT)
+#define	PLX_GPIO7_DIR		(1 << PLX_GPIO7_DIR_BIT)
+#define PLX_GPIO8_DIR		(1 << PLX_GPIO8_DIR_BIT)
+
+#define	PLX_TERM_ON			PLX_GPIO7
+#define	PLX_SLAVE_EN_N		PLX_GPIO5
+#define	PLX_MASTER_EN		PLX_GPIO6
+#define	PLX_SYNC_O_EN		PLX_GPIO4
+#define PLX_DSP_RES_N		PLX_GPIO8
+/* GPIO4..8 Enable & Set to OUT, SLAVE_EN_N = 1 */
+#define PLX_GPIOC_INIT		(PLX_GPIO4_DIR | PLX_GPIO5_DIR | PLX_GPIO6_DIR \
+			| PLX_GPIO7_DIR | PLX_GPIO8_DIR | PLX_SLAVE_EN_N)
+
+/* PLX Interrupt Control/STATUS */
+#define PLX_INTCSR_LINTI1_ENABLE 0x01
+#define PLX_INTCSR_LINTI1_STATUS 0x04
+#define PLX_INTCSR_LINTI2_ENABLE 0x08
+#define PLX_INTCSR_LINTI2_STATUS 0x20
+#define PLX_INTCSR_PCIINT_ENABLE 0x40
+
+/* PLX Registers */
+#define PLX_INTCSR 0x4c
+#define PLX_CNTRL  0x50
+#define PLX_GPIOC  0x54
+
+
+/*
+ * REGISTER SETTING FOR HFC-4S/8S AND HFC-E1
+ */
+
+/* write only registers */
+#define R_CIRM			0x00
+#define R_CTRL			0x01
+#define R_BRG_PCM_CFG 		0x02
+#define R_RAM_ADDR0		0x08
+#define R_RAM_ADDR1		0x09
+#define R_RAM_ADDR2		0x0A
+#define R_FIRST_FIFO		0x0B
+#define R_RAM_SZ		0x0C
+#define R_FIFO_MD		0x0D
+#define R_INC_RES_FIFO		0x0E
+#define R_FSM_IDX		0x0F
+#define R_FIFO			0x0F
+#define R_SLOT			0x10
+#define R_IRQMSK_MISC		0x11
+#define R_SCI_MSK		0x12
+#define R_IRQ_CTRL		0x13
+#define R_PCM_MD0		0x14
+#define R_PCM_MD1		0x15
+#define R_PCM_MD2		0x15
+#define R_SH0H			0x15
+#define R_SH1H			0x15
+#define R_SH0L			0x15
+#define R_SH1L			0x15
+#define R_SL_SEL0		0x15
+#define R_SL_SEL1		0x15
+#define R_SL_SEL2		0x15
+#define R_SL_SEL3		0x15
+#define R_SL_SEL4		0x15
+#define R_SL_SEL5		0x15
+#define R_SL_SEL6		0x15
+#define R_SL_SEL7		0x15
+#define R_ST_SEL		0x16
+#define R_ST_SYNC		0x17
+#define R_CONF_EN		0x18
+#define R_TI_WD			0x1A
+#define R_BERT_WD_MD		0x1B
+#define R_DTMF			0x1C
+#define R_DTMF_N		0x1D
+#define R_E1_WR_STA		0x20
+#define R_E1_RD_STA		0x20
+#define R_LOS0			0x22
+#define R_LOS1			0x23
+#define R_RX0			0x24
+#define R_RX_FR0		0x25
+#define R_RX_FR1		0x26
+#define R_TX0			0x28
+#define R_TX1			0x29
+#define R_TX_FR0		0x2C
+
+#define R_TX_FR1		0x2D
+#define R_TX_FR2		0x2E
+#define R_JATT_ATT		0x2F /* undocumented */
+#define A_ST_RD_STATE		0x30
+#define A_ST_WR_STATE		0x30
+#define R_RX_OFF		0x30
+#define A_ST_CTRL0		0x31
+#define R_SYNC_OUT		0x31
+#define A_ST_CTRL1		0x32
+#define A_ST_CTRL2		0x33
+#define A_ST_SQ_WR		0x34
+#define R_TX_OFF		0x34
+#define R_SYNC_CTRL		0x35
+#define A_ST_CLK_DLY		0x37
+#define R_PWM0			0x38
+#define R_PWM1			0x39
+#define A_ST_B1_TX		0x3C
+#define A_ST_B2_TX		0x3D
+#define A_ST_D_TX		0x3E
+#define R_GPIO_OUT0		0x40
+#define R_GPIO_OUT1		0x41
+#define R_GPIO_EN0		0x42
+#define R_GPIO_EN1		0x43
+#define R_GPIO_SEL		0x44
+#define R_BRG_CTRL		0x45
+#define R_PWM_MD		0x46
+#define R_BRG_MD		0x47
+#define R_BRG_TIM0		0x48
+#define R_BRG_TIM1		0x49
+#define R_BRG_TIM2		0x4A
+#define R_BRG_TIM3		0x4B
+#define R_BRG_TIM_SEL01		0x4C
+#define R_BRG_TIM_SEL23		0x4D
+#define R_BRG_TIM_SEL45		0x4E
+#define R_BRG_TIM_SEL67		0x4F
+#define A_SL_CFG		0xD0
+#define A_CONF			0xD1
+#define A_CH_MSK		0xF4
+#define A_CON_HDLC		0xFA
+#define A_SUBCH_CFG		0xFB
+#define A_CHANNEL		0xFC
+#define A_FIFO_SEQ		0xFD
+#define A_IRQ_MSK		0xFF
+
+/* read only registers */
+#define A_Z12			0x04
+#define A_Z1L			0x04
+#define A_Z1			0x04
+#define A_Z1H			0x05
+#define A_Z2L			0x06
+#define A_Z2			0x06
+#define A_Z2H			0x07
+#define A_F1			0x0C
+#define A_F12			0x0C
+#define A_F2			0x0D
+#define R_IRQ_OVIEW		0x10
+#define R_IRQ_MISC		0x11
+#define R_IRQ_STATECH		0x12
+#define R_CONF_OFLOW		0x14
+#define R_RAM_USE		0x15
+#define R_CHIP_ID		0x16
+#define R_BERT_STA		0x17
+#define R_F0_CNTL		0x18
+#define R_F0_CNTH		0x19
+#define R_BERT_EC		0x1A
+#define R_BERT_ECL		0x1A
+#define R_BERT_ECH		0x1B
+#define R_STATUS		0x1C
+#define R_CHIP_RV		0x1F
+#define R_STATE			0x20
+#define R_SYNC_STA		0x24
+#define R_RX_SL0_0		0x25
+#define R_RX_SL0_1		0x26
+#define R_RX_SL0_2		0x27
+#define R_JATT_DIR		0x2b /* undocumented */
+#define R_SLIP			0x2c
+#define A_ST_RD_STA		0x30
+#define R_FAS_EC		0x30
+#define R_FAS_ECL		0x30
+#define R_FAS_ECH		0x31
+#define R_VIO_EC		0x32
+#define R_VIO_ECL		0x32
+#define R_VIO_ECH		0x33
+#define A_ST_SQ_RD		0x34
+#define R_CRC_EC		0x34
+#define R_CRC_ECL		0x34
+#define R_CRC_ECH		0x35
+#define R_E_EC			0x36
+#define R_E_ECL			0x36
+#define R_E_ECH			0x37
+#define R_SA6_SA13_EC		0x38
+#define R_SA6_SA13_ECL		0x38
+#define R_SA6_SA13_ECH		0x39
+#define R_SA6_SA23_EC		0x3A
+#define R_SA6_SA23_ECL		0x3A
+#define R_SA6_SA23_ECH		0x3B
+#define A_ST_B1_RX		0x3C
+#define A_ST_B2_RX		0x3D
+#define A_ST_D_RX		0x3E
+#define A_ST_E_RX		0x3F
+#define R_GPIO_IN0		0x40
+#define R_GPIO_IN1		0x41
+#define R_GPI_IN0		0x44
+#define R_GPI_IN1		0x45
+#define R_GPI_IN2		0x46
+#define R_GPI_IN3		0x47
+#define R_INT_DATA		0x88
+#define R_IRQ_FIFO_BL0		0xC8
+#define R_IRQ_FIFO_BL1		0xC9
+#define R_IRQ_FIFO_BL2		0xCA
+#define R_IRQ_FIFO_BL3		0xCB
+#define R_IRQ_FIFO_BL4		0xCC
+#define R_IRQ_FIFO_BL5		0xCD
+#define R_IRQ_FIFO_BL6		0xCE
+#define R_IRQ_FIFO_BL7		0xCF
+
+/* read and write registers */
+#define A_FIFO_DATA0		0x80
+#define A_FIFO_DATA1		0x80
+#define A_FIFO_DATA2		0x80
+#define A_FIFO_DATA0_NOINC	0x84
+#define A_FIFO_DATA1_NOINC	0x84
+#define A_FIFO_DATA2_NOINC	0x84
+#define R_RAM_DATA		0xC0
+
+
+/*
+ * BIT SETTING FOR HFC-4S/8S AND HFC-E1
+ */
+
+/* chapter 2: universal bus interface */
+/* R_CIRM */
+#define V_IRQ_SEL		0x01
+#define V_SRES			0x08
+#define V_HFCRES		0x10
+#define V_PCMRES		0x20
+#define V_STRES			0x40
+#define V_ETRES			0x40
+#define V_RLD_EPR		0x80
+/* R_CTRL */
+#define V_FIFO_LPRIO		0x02
+#define V_SLOW_RD		0x04
+#define V_EXT_RAM		0x08
+#define V_CLK_OFF		0x20
+#define V_ST_CLK		0x40
+/* R_RAM_ADDR0 */
+#define V_RAM_ADDR2		0x01
+#define V_ADDR_RES		0x40
+#define V_ADDR_INC		0x80
+/* R_RAM_SZ */
+#define V_RAM_SZ		0x01
+#define V_PWM0_16KHZ		0x10
+#define V_PWM1_16KHZ		0x20
+#define V_FZ_MD			0x80
+/* R_CHIP_ID */
+#define V_PNP_IRQ		0x01
+#define V_CHIP_ID		0x10
+
+/* chapter 3: data flow */
+/* R_FIRST_FIFO */
+#define V_FIRST_FIRO_DIR	0x01
+#define V_FIRST_FIFO_NUM	0x02
+/* R_FIFO_MD */
+#define V_FIFO_MD		0x01
+#define V_CSM_MD		0x04
+#define V_FSM_MD		0x08
+#define V_FIFO_SZ		0x10
+/* R_FIFO */
+#define V_FIFO_DIR		0x01
+#define V_FIFO_NUM		0x02
+#define V_REV			0x80
+/* R_SLOT */
+#define V_SL_DIR		0x01
+#define V_SL_NUM		0x02
+/* A_SL_CFG */
+#define V_CH_DIR		0x01
+#define V_CH_SEL		0x02
+#define V_ROUTING		0x40
+/* A_CON_HDLC */
+#define V_IFF			0x01
+#define V_HDLC_TRP		0x02
+#define V_TRP_IRQ		0x04
+#define V_DATA_FLOW		0x20
+/* A_SUBCH_CFG */
+#define V_BIT_CNT		0x01
+#define V_START_BIT		0x08
+#define V_LOOP_FIFO		0x40
+#define V_INV_DATA		0x80
+/* A_CHANNEL */
+#define V_CH_DIR0		0x01
+#define V_CH_NUM0		0x02
+/* A_FIFO_SEQ */
+#define V_NEXT_FIFO_DIR		0x01
+#define V_NEXT_FIFO_NUM		0x02
+#define V_SEQ_END		0x40
+
+/* chapter 4: FIFO handling and HDLC controller */
+/* R_INC_RES_FIFO */
+#define V_INC_F			0x01
+#define V_RES_F			0x02
+#define V_RES_LOST		0x04
+
+/* chapter 5: S/T interface */
+/* R_SCI_MSK */
+#define V_SCI_MSK_ST0		0x01
+#define V_SCI_MSK_ST1		0x02
+#define V_SCI_MSK_ST2		0x04
+#define V_SCI_MSK_ST3		0x08
+#define V_SCI_MSK_ST4		0x10
+#define V_SCI_MSK_ST5		0x20
+#define V_SCI_MSK_ST6		0x40
+#define V_SCI_MSK_ST7		0x80
+/* R_ST_SEL */
+#define V_ST_SEL		0x01
+#define V_MULT_ST		0x08
+/* R_ST_SYNC */
+#define V_SYNC_SEL		0x01
+#define V_AUTO_SYNC		0x08
+/* A_ST_WR_STA */
+#define V_ST_SET_STA		0x01
+#define V_ST_LD_STA		0x10
+#define V_ST_ACT		0x20
+#define V_SET_G2_G3		0x80
+/* A_ST_CTRL0 */
+#define V_B1_EN			0x01
+#define V_B2_EN			0x02
+#define V_ST_MD			0x04
+#define V_D_PRIO		0x08
+#define V_SQ_EN			0x10
+#define V_96KHZ			0x20
+#define V_TX_LI			0x40
+#define V_ST_STOP		0x80
+/* A_ST_CTRL1 */
+#define V_G2_G3_EN		0x01
+#define V_D_HI			0x04
+#define V_E_IGNO		0x08
+#define V_E_LO			0x10
+#define V_B12_SWAP		0x80
+/* A_ST_CTRL2 */
+#define V_B1_RX_EN		0x01
+#define V_B2_RX_EN		0x02
+#define V_ST_TRIS		0x40
+/* A_ST_CLK_DLY */
+#define V_ST_CK_DLY		0x01
+#define V_ST_SMPL		0x10
+/* A_ST_D_TX */
+#define V_ST_D_TX		0x40
+/* R_IRQ_STATECH */
+#define V_SCI_ST0		0x01
+#define V_SCI_ST1		0x02
+#define V_SCI_ST2		0x04
+#define V_SCI_ST3		0x08
+#define V_SCI_ST4		0x10
+#define V_SCI_ST5		0x20
+#define V_SCI_ST6		0x40
+#define V_SCI_ST7		0x80
+/* A_ST_RD_STA */
+#define V_ST_STA		0x01
+#define V_FR_SYNC_ST		0x10
+#define V_TI2_EXP		0x20
+#define V_INFO0			0x40
+#define V_G2_G3			0x80
+/* A_ST_SQ_RD */
+#define V_ST_SQ			0x01
+#define V_MF_RX_RDY		0x10
+#define V_MF_TX_RDY		0x80
+/* A_ST_D_RX */
+#define V_ST_D_RX		0x40
+/* A_ST_E_RX */
+#define V_ST_E_RX		0x40
+
+/* chapter 5: E1 interface */
+/* R_E1_WR_STA */
+/* R_E1_RD_STA */
+#define V_E1_SET_STA		0x01
+#define V_E1_LD_STA		0x10
+/* R_RX0 */
+#define V_RX_CODE		0x01
+#define V_RX_FBAUD		0x04
+#define V_RX_CMI		0x08
+#define V_RX_INV_CMI		0x10
+#define V_RX_INV_CLK		0x20
+#define V_RX_INV_DATA		0x40
+#define V_AIS_ITU		0x80
+/* R_RX_FR0 */
+#define V_NO_INSYNC		0x01
+#define V_AUTO_RESYNC		0x02
+#define V_AUTO_RECO		0x04
+#define V_SWORD_COND		0x08
+#define V_SYNC_LOSS		0x10
+#define V_XCRC_SYNC		0x20
+#define V_MF_RESYNC		0x40
+#define V_RESYNC		0x80
+/* R_RX_FR1 */
+#define V_RX_MF			0x01
+#define V_RX_MF_SYNC		0x02
+#define V_RX_SL0_RAM		0x04
+#define V_ERR_SIM		0x20
+#define V_RES_NMF		0x40
+/* R_TX0 */
+#define V_TX_CODE		0x01
+#define V_TX_FBAUD		0x04
+#define V_TX_CMI_CODE		0x08
+#define V_TX_INV_CMI_CODE	0x10
+#define V_TX_INV_CLK		0x20
+#define V_TX_INV_DATA		0x40
+#define V_OUT_EN		0x80
+/* R_TX1 */
+#define V_INV_CLK		0x01
+#define V_EXCHG_DATA_LI		0x02
+#define V_AIS_OUT		0x04
+#define V_ATX			0x20
+#define V_NTRI			0x40
+#define V_AUTO_ERR_RES		0x80
+/* R_TX_FR0 */
+#define V_TRP_FAS		0x01
+#define V_TRP_NFAS		0x02
+#define V_TRP_RAL		0x04
+#define V_TRP_SA		0x08
+/* R_TX_FR1 */
+#define V_TX_FAS		0x01
+#define V_TX_NFAS		0x02
+#define V_TX_RAL		0x04
+#define V_TX_SA			0x08
+/* R_TX_FR2 */
+#define V_TX_MF			0x01
+#define V_TRP_SL0		0x02
+#define V_TX_SL0_RAM		0x04
+#define V_TX_E			0x10
+#define V_NEG_E			0x20
+#define V_XS12_ON		0x40
+#define V_XS15_ON		0x80
+/* R_RX_OFF */
+#define V_RX_SZ			0x01
+#define V_RX_INIT		0x04
+/* R_SYNC_OUT */
+#define V_SYNC_E1_RX		0x01
+#define V_IPATS0		0x20
+#define V_IPATS1		0x40
+#define V_IPATS2		0x80
+/* R_TX_OFF */
+#define V_TX_SZ			0x01
+#define V_TX_INIT		0x04
+/* R_SYNC_CTRL */
+#define V_EXT_CLK_SYNC		0x01
+#define V_SYNC_OFFS		0x02
+#define V_PCM_SYNC		0x04
+#define V_NEG_CLK		0x08
+#define V_HCLK			0x10
+/*
+#define V_JATT_AUTO_DEL		0x20
+#define V_JATT_AUTO		0x40
+*/
+#define V_JATT_OFF		0x80
+/* R_STATE */
+#define V_E1_STA		0x01
+#define V_ALT_FR_RX		0x40
+#define V_ALT_FR_TX		0x80
+/* R_SYNC_STA */
+#define V_RX_STA		0x01
+#define V_FR_SYNC_E1		0x04
+#define V_SIG_LOS		0x08
+#define V_MFA_STA		0x10
+#define V_AIS			0x40
+#define V_NO_MF_SYNC		0x80
+/* R_RX_SL0_0 */
+#define V_SI_FAS		0x01
+#define V_SI_NFAS		0x02
+#define V_A			0x04
+#define V_CRC_OK		0x08
+#define V_TX_E1			0x10
+#define V_TX_E2			0x20
+#define V_RX_E1			0x40
+#define V_RX_E2			0x80
+/* R_SLIP */
+#define V_SLIP_RX		0x01
+#define V_FOSLIP_RX		0x08
+#define V_SLIP_TX		0x10
+#define V_FOSLIP_TX		0x80
+
+/* chapter 6: PCM interface */
+/* R_PCM_MD0 */
+#define V_PCM_MD		0x01
+#define V_C4_POL		0x02
+#define V_F0_NEG		0x04
+#define V_F0_LEN		0x08
+#define V_PCM_ADDR		0x10
+/* R_SL_SEL0 */
+#define V_SL_SEL0		0x01
+#define V_SH_SEL0		0x80
+/* R_SL_SEL1 */
+#define V_SL_SEL1		0x01
+#define V_SH_SEL1		0x80
+/* R_SL_SEL2 */
+#define V_SL_SEL2		0x01
+#define V_SH_SEL2		0x80
+/* R_SL_SEL3 */
+#define V_SL_SEL3		0x01
+#define V_SH_SEL3		0x80
+/* R_SL_SEL4 */
+#define V_SL_SEL4		0x01
+#define V_SH_SEL4		0x80
+/* R_SL_SEL5 */
+#define V_SL_SEL5		0x01
+#define V_SH_SEL5		0x80
+/* R_SL_SEL6 */
+#define V_SL_SEL6		0x01
+#define V_SH_SEL6		0x80
+/* R_SL_SEL7 */
+#define V_SL_SEL7		0x01
+#define V_SH_SEL7		0x80
+/* R_PCM_MD1 */
+#define V_ODEC_CON		0x01
+#define V_PLL_ADJ		0x04
+#define V_PCM_DR		0x10
+#define V_PCM_LOOP		0x40
+/* R_PCM_MD2 */
+#define V_SYNC_PLL		0x02
+#define V_SYNC_SRC		0x04
+#define V_SYNC_OUT		0x08
+#define V_ICR_FR_TIME		0x40
+#define V_EN_PLL		0x80
+
+/* chapter 7: pulse width modulation */
+/* R_PWM_MD */
+#define V_EXT_IRQ_EN		0x08
+#define V_PWM0_MD		0x10
+#define V_PWM1_MD		0x40
+
+/* chapter 8: multiparty audio conferences */
+/* R_CONF_EN */
+#define V_CONF_EN		0x01
+#define V_ULAW			0x80
+/* A_CONF */
+#define V_CONF_NUM		0x01
+#define V_NOISE_SUPPR		0x08
+#define V_ATT_LEV		0x20
+#define V_CONF_SL		0x80
+/* R_CONF_OFLOW */
+#define V_CONF_OFLOW0		0x01
+#define V_CONF_OFLOW1		0x02
+#define V_CONF_OFLOW2		0x04
+#define V_CONF_OFLOW3		0x08
+#define V_CONF_OFLOW4		0x10
+#define V_CONF_OFLOW5		0x20
+#define V_CONF_OFLOW6		0x40
+#define V_CONF_OFLOW7		0x80
+
+/* chapter 9: DTMF contoller */
+/* R_DTMF0 */
+#define V_DTMF_EN		0x01
+#define V_HARM_SEL		0x02
+#define V_DTMF_RX_CH		0x04
+#define V_DTMF_STOP		0x08
+#define V_CHBL_SEL		0x10
+#define V_RST_DTMF		0x40
+#define V_ULAW_SEL		0x80
+
+/* chapter 10: BERT */
+/* R_BERT_WD_MD */
+#define V_PAT_SEQ		0x01
+#define V_BERT_ERR		0x08
+#define V_AUTO_WD_RES		0x20
+#define V_WD_RES		0x80
+/* R_BERT_STA */
+#define V_BERT_SYNC_SRC		0x01
+#define V_BERT_SYNC		0x10
+#define V_BERT_INV_DATA		0x20
+
+/* chapter 11: auxiliary interface */
+/* R_BRG_PCM_CFG */
+#define V_BRG_EN		0x01
+#define V_BRG_MD		0x02
+#define V_PCM_CLK		0x20
+#define V_ADDR_WRDLY		0x40
+/* R_BRG_CTRL */
+#define V_BRG_CS		0x01
+#define V_BRG_ADDR		0x08
+#define V_BRG_CS_SRC		0x80
+/* R_BRG_MD */
+#define V_BRG_MD0		0x01
+#define V_BRG_MD1		0x02
+#define V_BRG_MD2		0x04
+#define V_BRG_MD3		0x08
+#define V_BRG_MD4		0x10
+#define V_BRG_MD5		0x20
+#define V_BRG_MD6		0x40
+#define V_BRG_MD7		0x80
+/* R_BRG_TIM0 */
+#define V_BRG_TIM0_IDLE		0x01
+#define V_BRG_TIM0_CLK		0x10
+/* R_BRG_TIM1 */
+#define V_BRG_TIM1_IDLE		0x01
+#define V_BRG_TIM1_CLK		0x10
+/* R_BRG_TIM2 */
+#define V_BRG_TIM2_IDLE		0x01
+#define V_BRG_TIM2_CLK		0x10
+/* R_BRG_TIM3 */
+#define V_BRG_TIM3_IDLE		0x01
+#define V_BRG_TIM3_CLK		0x10
+/* R_BRG_TIM_SEL01 */
+#define V_BRG_WR_SEL0		0x01
+#define V_BRG_RD_SEL0		0x04
+#define V_BRG_WR_SEL1		0x10
+#define V_BRG_RD_SEL1		0x40
+/* R_BRG_TIM_SEL23 */
+#define V_BRG_WR_SEL2		0x01
+#define V_BRG_RD_SEL2		0x04
+#define V_BRG_WR_SEL3		0x10
+#define V_BRG_RD_SEL3		0x40
+/* R_BRG_TIM_SEL45 */
+#define V_BRG_WR_SEL4		0x01
+#define V_BRG_RD_SEL4		0x04
+#define V_BRG_WR_SEL5		0x10
+#define V_BRG_RD_SEL5		0x40
+/* R_BRG_TIM_SEL67 */
+#define V_BRG_WR_SEL6		0x01
+#define V_BRG_RD_SEL6		0x04
+#define V_BRG_WR_SEL7		0x10
+#define V_BRG_RD_SEL7		0x40
+
+/* chapter 12: clock, reset, interrupt, timer and watchdog */
+/* R_IRQMSK_MISC */
+#define V_STA_IRQMSK		0x01
+#define V_TI_IRQMSK		0x02
+#define V_PROC_IRQMSK		0x04
+#define V_DTMF_IRQMSK		0x08
+#define V_IRQ1S_MSK		0x10
+#define V_SA6_IRQMSK		0x20
+#define V_RX_EOMF_MSK		0x40
+#define V_TX_EOMF_MSK		0x80
+/* R_IRQ_CTRL */
+#define V_FIFO_IRQ		0x01
+#define V_GLOB_IRQ_EN		0x08
+#define V_IRQ_POL		0x10
+/* R_TI_WD */
+#define V_EV_TS			0x01
+#define V_WD_TS			0x10
+/* A_IRQ_MSK */
+#define V_IRQ			0x01
+#define V_BERT_EN		0x02
+#define V_MIX_IRQ		0x04
+/* R_IRQ_OVIEW */
+#define V_IRQ_FIFO_BL0		0x01
+#define V_IRQ_FIFO_BL1		0x02
+#define V_IRQ_FIFO_BL2		0x04
+#define V_IRQ_FIFO_BL3		0x08
+#define V_IRQ_FIFO_BL4		0x10
+#define V_IRQ_FIFO_BL5		0x20
+#define V_IRQ_FIFO_BL6		0x40
+#define V_IRQ_FIFO_BL7		0x80
+/* R_IRQ_MISC */
+#define V_STA_IRQ		0x01
+#define V_TI_IRQ		0x02
+#define V_IRQ_PROC		0x04
+#define V_DTMF_IRQ		0x08
+#define V_IRQ1S			0x10
+#define V_SA6_IRQ		0x20
+#define V_RX_EOMF		0x40
+#define V_TX_EOMF		0x80
+/* R_STATUS */
+#define V_BUSY			0x01
+#define V_PROC			0x02
+#define V_DTMF_STA		0x04
+#define V_LOST_STA		0x08
+#define V_SYNC_IN		0x10
+#define V_EXT_IRQSTA		0x20
+#define V_MISC_IRQSTA		0x40
+#define V_FR_IRQSTA		0x80
+/* R_IRQ_FIFO_BL0 */
+#define V_IRQ_FIFO0_TX		0x01
+#define V_IRQ_FIFO0_RX		0x02
+#define V_IRQ_FIFO1_TX		0x04
+#define V_IRQ_FIFO1_RX		0x08
+#define V_IRQ_FIFO2_TX		0x10
+#define V_IRQ_FIFO2_RX		0x20
+#define V_IRQ_FIFO3_TX		0x40
+#define V_IRQ_FIFO3_RX		0x80
+/* R_IRQ_FIFO_BL1 */
+#define V_IRQ_FIFO4_TX		0x01
+#define V_IRQ_FIFO4_RX		0x02
+#define V_IRQ_FIFO5_TX		0x04
+#define V_IRQ_FIFO5_RX		0x08
+#define V_IRQ_FIFO6_TX		0x10
+#define V_IRQ_FIFO6_RX		0x20
+#define V_IRQ_FIFO7_TX		0x40
+#define V_IRQ_FIFO7_RX		0x80
+/* R_IRQ_FIFO_BL2 */
+#define V_IRQ_FIFO8_TX		0x01
+#define V_IRQ_FIFO8_RX		0x02
+#define V_IRQ_FIFO9_TX		0x04
+#define V_IRQ_FIFO9_RX		0x08
+#define V_IRQ_FIFO10_TX		0x10
+#define V_IRQ_FIFO10_RX		0x20
+#define V_IRQ_FIFO11_TX		0x40
+#define V_IRQ_FIFO11_RX		0x80
+/* R_IRQ_FIFO_BL3 */
+#define V_IRQ_FIFO12_TX		0x01
+#define V_IRQ_FIFO12_RX		0x02
+#define V_IRQ_FIFO13_TX		0x04
+#define V_IRQ_FIFO13_RX		0x08
+#define V_IRQ_FIFO14_TX		0x10
+#define V_IRQ_FIFO14_RX		0x20
+#define V_IRQ_FIFO15_TX		0x40
+#define V_IRQ_FIFO15_RX		0x80
+/* R_IRQ_FIFO_BL4 */
+#define V_IRQ_FIFO16_TX		0x01
+#define V_IRQ_FIFO16_RX		0x02
+#define V_IRQ_FIFO17_TX		0x04
+#define V_IRQ_FIFO17_RX		0x08
+#define V_IRQ_FIFO18_TX		0x10
+#define V_IRQ_FIFO18_RX		0x20
+#define V_IRQ_FIFO19_TX		0x40
+#define V_IRQ_FIFO19_RX		0x80
+/* R_IRQ_FIFO_BL5 */
+#define V_IRQ_FIFO20_TX		0x01
+#define V_IRQ_FIFO20_RX		0x02
+#define V_IRQ_FIFO21_TX		0x04
+#define V_IRQ_FIFO21_RX		0x08
+#define V_IRQ_FIFO22_TX		0x10
+#define V_IRQ_FIFO22_RX		0x20
+#define V_IRQ_FIFO23_TX		0x40
+#define V_IRQ_FIFO23_RX		0x80
+/* R_IRQ_FIFO_BL6 */
+#define V_IRQ_FIFO24_TX		0x01
+#define V_IRQ_FIFO24_RX		0x02
+#define V_IRQ_FIFO25_TX		0x04
+#define V_IRQ_FIFO25_RX		0x08
+#define V_IRQ_FIFO26_TX		0x10
+#define V_IRQ_FIFO26_RX		0x20
+#define V_IRQ_FIFO27_TX		0x40
+#define V_IRQ_FIFO27_RX		0x80
+/* R_IRQ_FIFO_BL7 */
+#define V_IRQ_FIFO28_TX		0x01
+#define V_IRQ_FIFO28_RX		0x02
+#define V_IRQ_FIFO29_TX		0x04
+#define V_IRQ_FIFO29_RX		0x08
+#define V_IRQ_FIFO30_TX		0x10
+#define V_IRQ_FIFO30_RX		0x20
+#define V_IRQ_FIFO31_TX		0x40
+#define V_IRQ_FIFO31_RX		0x80
+
+/* chapter 13: general purpose I/O pins (GPIO) and input pins (GPI) */
+/* R_GPIO_OUT0 */
+#define V_GPIO_OUT0		0x01
+#define V_GPIO_OUT1		0x02
+#define V_GPIO_OUT2		0x04
+#define V_GPIO_OUT3		0x08
+#define V_GPIO_OUT4		0x10
+#define V_GPIO_OUT5		0x20
+#define V_GPIO_OUT6		0x40
+#define V_GPIO_OUT7		0x80
+/* R_GPIO_OUT1 */
+#define V_GPIO_OUT8		0x01
+#define V_GPIO_OUT9		0x02
+#define V_GPIO_OUT10		0x04
+#define V_GPIO_OUT11		0x08
+#define V_GPIO_OUT12		0x10
+#define V_GPIO_OUT13		0x20
+#define V_GPIO_OUT14		0x40
+#define V_GPIO_OUT15		0x80
+/* R_GPIO_EN0 */
+#define V_GPIO_EN0		0x01
+#define V_GPIO_EN1		0x02
+#define V_GPIO_EN2		0x04
+#define V_GPIO_EN3		0x08
+#define V_GPIO_EN4		0x10
+#define V_GPIO_EN5		0x20
+#define V_GPIO_EN6		0x40
+#define V_GPIO_EN7		0x80
+/* R_GPIO_EN1 */
+#define V_GPIO_EN8		0x01
+#define V_GPIO_EN9		0x02
+#define V_GPIO_EN10		0x04
+#define V_GPIO_EN11		0x08
+#define V_GPIO_EN12		0x10
+#define V_GPIO_EN13		0x20
+#define V_GPIO_EN14		0x40
+#define V_GPIO_EN15		0x80
+/* R_GPIO_SEL */
+#define V_GPIO_SEL0		0x01
+#define V_GPIO_SEL1		0x02
+#define V_GPIO_SEL2		0x04
+#define V_GPIO_SEL3		0x08
+#define V_GPIO_SEL4		0x10
+#define V_GPIO_SEL5		0x20
+#define V_GPIO_SEL6		0x40
+#define V_GPIO_SEL7		0x80
+/* R_GPIO_IN0 */
+#define V_GPIO_IN0		0x01
+#define V_GPIO_IN1		0x02
+#define V_GPIO_IN2		0x04
+#define V_GPIO_IN3		0x08
+#define V_GPIO_IN4		0x10
+#define V_GPIO_IN5		0x20
+#define V_GPIO_IN6		0x40
+#define V_GPIO_IN7		0x80
+/* R_GPIO_IN1 */
+#define V_GPIO_IN8		0x01
+#define V_GPIO_IN9		0x02
+#define V_GPIO_IN10		0x04
+#define V_GPIO_IN11		0x08
+#define V_GPIO_IN12		0x10
+#define V_GPIO_IN13		0x20
+#define V_GPIO_IN14		0x40
+#define V_GPIO_IN15		0x80
+/* R_GPI_IN0 */
+#define V_GPI_IN0		0x01
+#define V_GPI_IN1		0x02
+#define V_GPI_IN2		0x04
+#define V_GPI_IN3		0x08
+#define V_GPI_IN4		0x10
+#define V_GPI_IN5		0x20
+#define V_GPI_IN6		0x40
+#define V_GPI_IN7		0x80
+/* R_GPI_IN1 */
+#define V_GPI_IN8		0x01
+#define V_GPI_IN9		0x02
+#define V_GPI_IN10		0x04
+#define V_GPI_IN11		0x08
+#define V_GPI_IN12		0x10
+#define V_GPI_IN13		0x20
+#define V_GPI_IN14		0x40
+#define V_GPI_IN15		0x80
+/* R_GPI_IN2 */
+#define V_GPI_IN16		0x01
+#define V_GPI_IN17		0x02
+#define V_GPI_IN18		0x04
+#define V_GPI_IN19		0x08
+#define V_GPI_IN20		0x10
+#define V_GPI_IN21		0x20
+#define V_GPI_IN22		0x40
+#define V_GPI_IN23		0x80
+/* R_GPI_IN3 */
+#define V_GPI_IN24		0x01
+#define V_GPI_IN25		0x02
+#define V_GPI_IN26		0x04
+#define V_GPI_IN27		0x08
+#define V_GPI_IN28		0x10
+#define V_GPI_IN29		0x20
+#define V_GPI_IN30		0x40
+#define V_GPI_IN31		0x80
+
+/* map of all registers, used for debugging */
+
+#ifdef HFC_REGISTER_DEBUG
+struct hfc_register_names {
+	char *name;
+	u_char reg;
+} hfc_register_names[] = {
+	/* write registers */
+	{"R_CIRM",		0x00},
+	{"R_CTRL",		0x01},
+	{"R_BRG_PCM_CFG ",	0x02},
+	{"R_RAM_ADDR0",		0x08},
+	{"R_RAM_ADDR1",		0x09},
+	{"R_RAM_ADDR2",		0x0A},
+	{"R_FIRST_FIFO",	0x0B},
+	{"R_RAM_SZ",		0x0C},
+	{"R_FIFO_MD",		0x0D},
+	{"R_INC_RES_FIFO",	0x0E},
+	{"R_FIFO / R_FSM_IDX",	0x0F},
+	{"R_SLOT",		0x10},
+	{"R_IRQMSK_MISC",	0x11},
+	{"R_SCI_MSK",		0x12},
+	{"R_IRQ_CTRL",		0x13},
+	{"R_PCM_MD0",		0x14},
+	{"R_0x15",		0x15},
+	{"R_ST_SEL",		0x16},
+	{"R_ST_SYNC",		0x17},
+	{"R_CONF_EN",		0x18},
+	{"R_TI_WD",		0x1A},
+	{"R_BERT_WD_MD",	0x1B},
+	{"R_DTMF",		0x1C},
+	{"R_DTMF_N",		0x1D},
+	{"R_E1_XX_STA",		0x20},
+	{"R_LOS0",		0x22},
+	{"R_LOS1",		0x23},
+	{"R_RX0",		0x24},
+	{"R_RX_FR0",		0x25},
+	{"R_RX_FR1",		0x26},
+	{"R_TX0",		0x28},
+	{"R_TX1",		0x29},
+	{"R_TX_FR0",		0x2C},
+	{"R_TX_FR1",		0x2D},
+	{"R_TX_FR2",		0x2E},
+	{"R_JATT_ATT",		0x2F},
+	{"A_ST_xx_STA/R_RX_OFF", 0x30},
+	{"A_ST_CTRL0/R_SYNC_OUT", 0x31},
+	{"A_ST_CTRL1",		0x32},
+	{"A_ST_CTRL2",		0x33},
+	{"A_ST_SQ_WR",		0x34},
+	{"R_TX_OFF",		0x34},
+	{"R_SYNC_CTRL",		0x35},
+	{"A_ST_CLK_DLY",	0x37},
+	{"R_PWM0",		0x38},
+	{"R_PWM1",		0x39},
+	{"A_ST_B1_TX",		0x3C},
+	{"A_ST_B2_TX",		0x3D},
+	{"A_ST_D_TX",		0x3E},
+	{"R_GPIO_OUT0",		0x40},
+	{"R_GPIO_OUT1",		0x41},
+	{"R_GPIO_EN0",		0x42},
+	{"R_GPIO_EN1",		0x43},
+	{"R_GPIO_SEL",		0x44},
+	{"R_BRG_CTRL",		0x45},
+	{"R_PWM_MD",		0x46},
+	{"R_BRG_MD",		0x47},
+	{"R_BRG_TIM0",		0x48},
+	{"R_BRG_TIM1",		0x49},
+	{"R_BRG_TIM2",		0x4A},
+	{"R_BRG_TIM3",		0x4B},
+	{"R_BRG_TIM_SEL01",	0x4C},
+	{"R_BRG_TIM_SEL23",	0x4D},
+	{"R_BRG_TIM_SEL45",	0x4E},
+	{"R_BRG_TIM_SEL67",	0x4F},
+	{"A_FIFO_DATA0-2",	0x80},
+	{"A_FIFO_DATA0-2_NOINC", 0x84},
+	{"R_RAM_DATA",		0xC0},
+	{"A_SL_CFG",		0xD0},
+	{"A_CONF",		0xD1},
+	{"A_CH_MSK",		0xF4},
+	{"A_CON_HDLC",		0xFA},
+	{"A_SUBCH_CFG",		0xFB},
+	{"A_CHANNEL",		0xFC},
+	{"A_FIFO_SEQ",		0xFD},
+	{"A_IRQ_MSK",		0xFF},
+	{NULL, 0},
+
+	/* read registers */
+	{"A_Z1",		0x04},
+	{"A_Z1H",		0x05},
+	{"A_Z2",		0x06},
+	{"A_Z2H",		0x07},
+	{"A_F1",		0x0C},
+	{"A_F2",		0x0D},
+	{"R_IRQ_OVIEW",		0x10},
+	{"R_IRQ_MISC",		0x11},
+	{"R_IRQ_STATECH",	0x12},
+	{"R_CONF_OFLOW",	0x14},
+	{"R_RAM_USE",		0x15},
+	{"R_CHIP_ID",		0x16},
+	{"R_BERT_STA",		0x17},
+	{"R_F0_CNTL",		0x18},
+	{"R_F0_CNTH",		0x19},
+	{"R_BERT_ECL",		0x1A},
+	{"R_BERT_ECH",		0x1B},
+	{"R_STATUS",		0x1C},
+	{"R_CHIP_RV",		0x1F},
+	{"R_STATE",		0x20},
+	{"R_SYNC_STA",		0x24},
+	{"R_RX_SL0_0",		0x25},
+	{"R_RX_SL0_1",		0x26},
+	{"R_RX_SL0_2",		0x27},
+	{"R_JATT_DIR",		0x2b},
+	{"R_SLIP",		0x2c},
+	{"A_ST_RD_STA",		0x30},
+	{"R_FAS_ECL",		0x30},
+	{"R_FAS_ECH",		0x31},
+	{"R_VIO_ECL",		0x32},
+	{"R_VIO_ECH",		0x33},
+	{"R_CRC_ECL / A_ST_SQ_RD", 0x34},
+	{"R_CRC_ECH",		0x35},
+	{"R_E_ECL",		0x36},
+	{"R_E_ECH",		0x37},
+	{"R_SA6_SA13_ECL",	0x38},
+	{"R_SA6_SA13_ECH",	0x39},
+	{"R_SA6_SA23_ECL",	0x3A},
+	{"R_SA6_SA23_ECH",	0x3B},
+	{"A_ST_B1_RX",		0x3C},
+	{"A_ST_B2_RX",		0x3D},
+	{"A_ST_D_RX",		0x3E},
+	{"A_ST_E_RX",		0x3F},
+	{"R_GPIO_IN0",		0x40},
+	{"R_GPIO_IN1",		0x41},
+	{"R_GPI_IN0",		0x44},
+	{"R_GPI_IN1",		0x45},
+	{"R_GPI_IN2",		0x46},
+	{"R_GPI_IN3",		0x47},
+	{"A_FIFO_DATA0-2",	0x80},
+	{"A_FIFO_DATA0-2_NOINC", 0x84},
+	{"R_INT_DATA",		0x88},
+	{"R_RAM_DATA",		0xC0},
+	{"R_IRQ_FIFO_BL0",	0xC8},
+	{"R_IRQ_FIFO_BL1",	0xC9},
+	{"R_IRQ_FIFO_BL2",	0xCA},
+	{"R_IRQ_FIFO_BL3",	0xCB},
+	{"R_IRQ_FIFO_BL4",	0xCC},
+	{"R_IRQ_FIFO_BL5",	0xCD},
+	{"R_IRQ_FIFO_BL6",	0xCE},
+	{"R_IRQ_FIFO_BL7",	0xCF},
+};
+#endif /* HFC_REGISTER_DEBUG */
+
diff --git a/drivers/isdn/hardware/mISDN/hfc_pci.h b/drivers/isdn/hardware/mISDN/hfc_pci.h
new file mode 100644
index 00000000000..fd2c9be6d84
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/hfc_pci.h
@@ -0,0 +1,228 @@
+/*
+ *  specific defines for CCD's HFC 2BDS0 PCI chips
+ *
+ * Author     Werner Cornelius (werner@isdn4linux.de)
+ *
+ * Copyright 1999  by Werner Cornelius (werner@isdn4linux.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, 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.
+ *
+ */
+
+/*
+ * thresholds for transparent B-channel mode
+ * change mask and threshold simultaneously
+ */
+#define HFCPCI_BTRANS_THRESHOLD 128
+#define HFCPCI_BTRANS_MAX	256
+#define HFCPCI_BTRANS_THRESMASK 0x00
+
+/* defines for PCI config */
+#define PCI_ENA_MEMIO		0x02
+#define PCI_ENA_MASTER		0x04
+
+/* GCI/IOM bus monitor registers */
+#define HCFPCI_C_I		0x08
+#define HFCPCI_TRxR		0x0C
+#define HFCPCI_MON1_D		0x28
+#define HFCPCI_MON2_D		0x2C
+
+/* GCI/IOM bus timeslot registers */
+#define HFCPCI_B1_SSL		0x80
+#define HFCPCI_B2_SSL		0x84
+#define HFCPCI_AUX1_SSL		0x88
+#define HFCPCI_AUX2_SSL		0x8C
+#define HFCPCI_B1_RSL		0x90
+#define HFCPCI_B2_RSL		0x94
+#define HFCPCI_AUX1_RSL		0x98
+#define HFCPCI_AUX2_RSL		0x9C
+
+/* GCI/IOM bus data registers */
+#define HFCPCI_B1_D		0xA0
+#define HFCPCI_B2_D		0xA4
+#define HFCPCI_AUX1_D		0xA8
+#define HFCPCI_AUX2_D		0xAC
+
+/* GCI/IOM bus configuration registers */
+#define HFCPCI_MST_EMOD		0xB4
+#define HFCPCI_MST_MODE		0xB8
+#define HFCPCI_CONNECT 		0xBC
+
+
+/* Interrupt and status registers */
+#define HFCPCI_FIFO_EN		0x44
+#define HFCPCI_TRM		0x48
+#define HFCPCI_B_MODE		0x4C
+#define HFCPCI_CHIP_ID		0x58
+#define HFCPCI_CIRM		0x60
+#define HFCPCI_CTMT		0x64
+#define HFCPCI_INT_M1		0x68
+#define HFCPCI_INT_M2		0x6C
+#define HFCPCI_INT_S1		0x78
+#define HFCPCI_INT_S2		0x7C
+#define HFCPCI_STATUS		0x70
+
+/* S/T section registers */
+#define HFCPCI_STATES		0xC0
+#define HFCPCI_SCTRL		0xC4
+#define HFCPCI_SCTRL_E		0xC8
+#define HFCPCI_SCTRL_R		0xCC
+#define HFCPCI_SQ		0xD0
+#define HFCPCI_CLKDEL		0xDC
+#define HFCPCI_B1_REC		0xF0
+#define HFCPCI_B1_SEND		0xF0
+#define HFCPCI_B2_REC		0xF4
+#define HFCPCI_B2_SEND		0xF4
+#define HFCPCI_D_REC		0xF8
+#define HFCPCI_D_SEND		0xF8
+#define HFCPCI_E_REC		0xFC
+
+
+/* bits in status register (READ) */
+#define HFCPCI_PCI_PROC		0x02
+#define HFCPCI_NBUSY		0x04
+#define HFCPCI_TIMER_ELAP	0x10
+#define HFCPCI_STATINT		0x20
+#define HFCPCI_FRAMEINT		0x40
+#define HFCPCI_ANYINT		0x80
+
+/* bits in CTMT (Write) */
+#define HFCPCI_CLTIMER		0x80
+#define HFCPCI_TIM3_125		0x04
+#define HFCPCI_TIM25		0x10
+#define HFCPCI_TIM50		0x14
+#define HFCPCI_TIM400		0x18
+#define HFCPCI_TIM800		0x1C
+#define HFCPCI_AUTO_TIMER	0x20
+#define HFCPCI_TRANSB2		0x02
+#define HFCPCI_TRANSB1		0x01
+
+/* bits in CIRM (Write) */
+#define HFCPCI_AUX_MSK		0x07
+#define HFCPCI_RESET		0x08
+#define HFCPCI_B1_REV		0x40
+#define HFCPCI_B2_REV		0x80
+
+/* bits in INT_M1 and INT_S1 */
+#define HFCPCI_INTS_B1TRANS	0x01
+#define HFCPCI_INTS_B2TRANS	0x02
+#define HFCPCI_INTS_DTRANS	0x04
+#define HFCPCI_INTS_B1REC	0x08
+#define HFCPCI_INTS_B2REC	0x10
+#define HFCPCI_INTS_DREC	0x20
+#define HFCPCI_INTS_L1STATE	0x40
+#define HFCPCI_INTS_TIMER	0x80
+
+/* bits in INT_M2 */
+#define HFCPCI_PROC_TRANS	0x01
+#define HFCPCI_GCI_I_CHG	0x02
+#define HFCPCI_GCI_MON_REC	0x04
+#define HFCPCI_IRQ_ENABLE	0x08
+#define HFCPCI_PMESEL		0x80
+
+/* bits in STATES */
+#define HFCPCI_STATE_MSK	0x0F
+#define HFCPCI_LOAD_STATE	0x10
+#define HFCPCI_ACTIVATE		0x20
+#define HFCPCI_DO_ACTION	0x40
+#define HFCPCI_NT_G2_G3		0x80
+
+/* bits in HFCD_MST_MODE */
+#define HFCPCI_MASTER		0x01
+#define HFCPCI_SLAVE		0x00
+#define HFCPCI_F0IO_POSITIV	0x02
+#define HFCPCI_F0_NEGATIV	0x04
+#define HFCPCI_F0_2C4		0x08
+/* remaining bits are for codecs control */
+
+/* bits in HFCD_SCTRL */
+#define SCTRL_B1_ENA		0x01
+#define SCTRL_B2_ENA		0x02
+#define SCTRL_MODE_TE		0x00
+#define SCTRL_MODE_NT		0x04
+#define SCTRL_LOW_PRIO		0x08
+#define SCTRL_SQ_ENA		0x10
+#define SCTRL_TEST		0x20
+#define SCTRL_NONE_CAP		0x40
+#define SCTRL_PWR_DOWN		0x80
+
+/* bits in SCTRL_E  */
+#define HFCPCI_AUTO_AWAKE	0x01
+#define HFCPCI_DBIT_1		0x04
+#define HFCPCI_IGNORE_COL	0x08
+#define HFCPCI_CHG_B1_B2	0x80
+
+/* bits in FIFO_EN register */
+#define HFCPCI_FIFOEN_B1	0x03
+#define HFCPCI_FIFOEN_B2	0x0C
+#define HFCPCI_FIFOEN_DTX	0x10
+#define HFCPCI_FIFOEN_B1TX	0x01
+#define HFCPCI_FIFOEN_B1RX	0x02
+#define HFCPCI_FIFOEN_B2TX	0x04
+#define HFCPCI_FIFOEN_B2RX	0x08
+
+
+/* definitions of fifo memory area */
+#define MAX_D_FRAMES 15
+#define MAX_B_FRAMES 31
+#define B_SUB_VAL    0x200
+#define B_FIFO_SIZE  (0x2000 - B_SUB_VAL)
+#define D_FIFO_SIZE  512
+#define D_FREG_MASK  0xF
+
+struct zt {
+	unsigned short z1;  /* Z1 pointer 16 Bit */
+	unsigned short z2;  /* Z2 pointer 16 Bit */
+};
+
+struct dfifo {
+	u_char data[D_FIFO_SIZE]; /* FIFO data space */
+	u_char fill1[0x20A0-D_FIFO_SIZE]; /* reserved, do not use */
+	u_char f1, f2; /* f pointers */
+	u_char fill2[0x20C0-0x20A2]; /* reserved, do not use */
+	/* mask index with D_FREG_MASK for access */
+	struct zt za[MAX_D_FRAMES+1];
+	u_char fill3[0x4000-0x2100]; /* align 16K */
+};
+
+struct bzfifo {
+	struct zt	za[MAX_B_FRAMES+1]; /* only range 0x0..0x1F allowed */
+	u_char		f1, f2; /* f pointers */
+	u_char		fill[0x2100-0x2082]; /* alignment */
+};
+
+
+union fifo_area {
+	struct {
+		struct dfifo d_tx; /* D-send channel */
+		struct dfifo d_rx; /* D-receive channel */
+	} d_chan;
+	struct {
+		u_char		fill1[0x200];
+		u_char		txdat_b1[B_FIFO_SIZE];
+		struct bzfifo	txbz_b1;
+		struct bzfifo	txbz_b2;
+		u_char		txdat_b2[B_FIFO_SIZE];
+		u_char		fill2[D_FIFO_SIZE];
+		u_char		rxdat_b1[B_FIFO_SIZE];
+		struct bzfifo	rxbz_b1;
+		struct bzfifo	rxbz_b2;
+		u_char rxdat_b2[B_FIFO_SIZE];
+	} b_chans;
+	u_char fill[32768];
+};
+
+#define Write_hfc(a, b, c) (writeb(c, (a->hw.pci_io)+b))
+#define Read_hfc(a, b) (readb((a->hw.pci_io)+b))
diff --git a/drivers/isdn/hardware/mISDN/hfcmulti.c b/drivers/isdn/hardware/mISDN/hfcmulti.c
new file mode 100644
index 00000000000..2649ea55a9e
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/hfcmulti.c
@@ -0,0 +1,5320 @@
+/*
+ * hfcmulti.c  low level driver for hfc-4s/hfc-8s/hfc-e1 based cards
+ *
+ * Author	Andreas Eversberg (jolly@eversberg.eu)
+ * ported to mqueue mechanism:
+ *		Peter Sprenger (sprengermoving-bytes.de)
+ *
+ * inspired by existing hfc-pci driver:
+ * Copyright 1999  by Werner Cornelius (werner@isdn-development.de)
+ * Copyright 2008  by Karsten Keil (kkeil@suse.de)
+ * Copyright 2008  by Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This program is free software; 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.
+ *
+ *
+ * Thanks to Cologne Chip AG for this great controller!
+ */
+
+/*
+ * module parameters:
+ * type:
+ *	By default (0), the card is automatically detected.
+ *	Or use the following combinations:
+ *	Bit 0-7   = 0x00001 = HFC-E1 (1 port)
+ * or	Bit 0-7   = 0x00004 = HFC-4S (4 ports)
+ * or	Bit 0-7   = 0x00008 = HFC-8S (8 ports)
+ *	Bit 8     = 0x00100 = uLaw (instead of aLaw)
+ *	Bit 9     = 0x00200 = Disable DTMF detect on all B-channels via hardware
+ *	Bit 10    = spare
+ *	Bit 11    = 0x00800 = Force PCM bus into slave mode. (otherwhise auto)
+ * or   Bit 12    = 0x01000 = Force PCM bus into master mode. (otherwhise auto)
+ *	Bit 13	  = spare
+ *	Bit 14    = 0x04000 = Use external ram (128K)
+ *	Bit 15    = 0x08000 = Use external ram (512K)
+ *	Bit 16    = 0x10000 = Use 64 timeslots instead of 32
+ * or	Bit 17    = 0x20000 = Use 128 timeslots instead of anything else
+ *	Bit 18    = spare
+ *	Bit 19    = 0x80000 = Send the Watchdog a Signal (Dual E1 with Watchdog)
+ * (all other bits are reserved and shall be 0)
+ *	example: 0x20204 one HFC-4S with dtmf detection and 128 timeslots on PCM
+ *		 bus (PCM master)
+ *
+ * port: (optional or required for all ports on all installed cards)
+ *	HFC-4S/HFC-8S only bits:
+ *	Bit 0	  = 0x001 = Use master clock for this S/T interface
+ *			    (ony once per chip).
+ *	Bit 1     = 0x002 = transmitter line setup (non capacitive mode)
+ *			    Don't use this unless you know what you are doing!
+ *	Bit 2     = 0x004 = Disable E-channel. (No E-channel processing)
+ *	example: 0x0001,0x0000,0x0000,0x0000 one HFC-4S with master clock
+ *		 received from port 1
+ *
+ *	HFC-E1 only bits:
+ *	Bit 0     = 0x0001 = interface: 0=copper, 1=optical
+ *	Bit 1     = 0x0002 = reserved (later for 32 B-channels transparent mode)
+ *	Bit 2     = 0x0004 = Report LOS
+ *	Bit 3     = 0x0008 = Report AIS
+ *	Bit 4     = 0x0010 = Report SLIP
+ *	Bit 5     = 0x0020 = Report RDI
+ *	Bit 8     = 0x0100 = Turn off CRC-4 Multiframe Mode, use double frame
+ *			     mode instead.
+ *	Bit 9	  = 0x0200 = Force get clock from interface, even in NT mode.
+ * or	Bit 10	  = 0x0400 = Force put clock to interface, even in TE mode.
+ *	Bit 11    = 0x0800 = Use direct RX clock for PCM sync rather than PLL.
+ *			     (E1 only)
+ *	Bit 12-13 = 0xX000 = elastic jitter buffer (1-3), Set both bits to 0
+ *			     for default.
+ * (all other bits are reserved and shall be 0)
+ *
+ * debug:
+ *	NOTE: only one debug value must be given for all cards
+ *	enable debugging (see hfc_multi.h for debug options)
+ *
+ * poll:
+ *	NOTE: only one poll value must be given for all cards
+ *	Give the number of samples for each fifo process.
+ *	By default 128 is used. Decrease to reduce delay, increase to
+ *	reduce cpu load. If unsure, don't mess with it!
+ *	Valid is 8, 16, 32, 64, 128, 256.
+ *
+ * pcm:
+ *	NOTE: only one pcm value must be given for every card.
+ *	The PCM bus id tells the mISDNdsp module about the connected PCM bus.
+ *	By default (0), the PCM bus id is 100 for the card that is PCM master.
+ *	If multiple cards are PCM master (because they are not interconnected),
+ *	each card with PCM master will have increasing PCM id.
+ *	All PCM busses with the same ID are expected to be connected and have
+ *	common time slots slots.
+ *	Only one chip of the PCM bus must be master, the others slave.
+ *	-1 means no support of PCM bus not even.
+ *	Omit this value, if all cards are interconnected or none is connected.
+ *	If unsure, don't give this parameter.
+ *
+ * dslot:
+ *	NOTE: only one poll value must be given for every card.
+ *	Also this value must be given for non-E1 cards. If omitted, the E1
+ *	card has D-channel on time slot 16, which is default.
+ *	If 1..15 or 17..31, an alternate time slot is used for D-channel.
+ *	In this case, the application must be able to handle this.
+ *	If -1 is given, the D-channel is disabled and all 31 slots can be used
+ *	for B-channel. (only for specific applications)
+ *	If you don't know how to use it, you don't need it!
+ *
+ * iomode:
+ *	NOTE: only one mode value must be given for every card.
+ *	-> See hfc_multi.h for HFC_IO_MODE_* values
+ *	By default, the IO mode is pci memory IO (MEMIO).
+ *	Some cards requre specific IO mode, so it cannot be changed.
+ *	It may be usefull to set IO mode to register io (REGIO) to solve
+ *	PCI bridge problems.
+ *	If unsure, don't give this parameter.
+ *
+ * clockdelay_nt:
+ *	NOTE: only one clockdelay_nt value must be given once for all cards.
+ *	Give the value of the clock control register (A_ST_CLK_DLY)
+ *	of the S/T interfaces in NT mode.
+ *	This register is needed for the TBR3 certification, so don't change it.
+ *
+ * clockdelay_te:
+ *	NOTE: only one clockdelay_te value must be given once
+ *	Give the value of the clock control register (A_ST_CLK_DLY)
+ *	of the S/T interfaces in TE mode.
+ *	This register is needed for the TBR3 certification, so don't change it.
+ */
+
+/*
+ * debug register access (never use this, it will flood your system log)
+ * #define HFC_REGISTER_DEBUG
+ */
+
+static const char *hfcmulti_revision = "2.00";
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/mISDNhw.h>
+#include <linux/mISDNdsp.h>
+
+/*
+#define IRQCOUNT_DEBUG
+#define IRQ_DEBUG
+*/
+
+#include "hfc_multi.h"
+#ifdef ECHOPREP
+#include "gaintab.h"
+#endif
+
+#define	MAX_CARDS	8
+#define	MAX_PORTS	(8 * MAX_CARDS)
+
+static LIST_HEAD(HFClist);
+static spinlock_t HFClock; /* global hfc list lock */
+
+static void ph_state_change(struct dchannel *);
+static void (*hfc_interrupt)(void);
+static void (*register_interrupt)(void);
+static int (*unregister_interrupt)(void);
+static int interrupt_registered;
+
+static struct hfc_multi *syncmaster;
+int plxsd_master; /* if we have a master card (yet) */
+static spinlock_t plx_lock; /* may not acquire other lock inside */
+EXPORT_SYMBOL(plx_lock);
+
+#define	TYP_E1		1
+#define	TYP_4S		4
+#define TYP_8S		8
+
+static int poll_timer = 6;	/* default = 128 samples = 16ms */
+/* number of POLL_TIMER interrupts for G2 timeout (ca 1s) */
+static int nt_t1_count[] = { 3840, 1920, 960, 480, 240, 120, 60, 30  };
+#define	CLKDEL_TE	0x0f	/* CLKDEL in TE mode */
+#define	CLKDEL_NT	0x6c	/* CLKDEL in NT mode
+				   (0x60 MUST be included!) */
+static u_char silence =	0xff;	/* silence by LAW */
+
+#define	DIP_4S	0x1		/* DIP Switches for Beronet 1S/2S/4S cards */
+#define	DIP_8S	0x2		/* DIP Switches for Beronet 8S+ cards */
+#define	DIP_E1	0x3		/* DIP Switches for Beronet E1 cards */
+
+/*
+ * module stuff
+ */
+
+static uint	type[MAX_CARDS];
+static uint	pcm[MAX_CARDS];
+static uint	dslot[MAX_CARDS];
+static uint	iomode[MAX_CARDS];
+static uint	port[MAX_PORTS];
+static uint	debug;
+static uint	poll;
+static uint	timer;
+static uint	clockdelay_te = CLKDEL_TE;
+static uint	clockdelay_nt = CLKDEL_NT;
+
+static int	HFC_cnt, Port_cnt, PCM_cnt = 99;
+
+MODULE_AUTHOR("Andreas Eversberg");
+MODULE_LICENSE("GPL");
+module_param(debug, uint, S_IRUGO | S_IWUSR);
+module_param(poll, uint, S_IRUGO | S_IWUSR);
+module_param(timer, uint, S_IRUGO | S_IWUSR);
+module_param(clockdelay_te, uint, S_IRUGO | S_IWUSR);
+module_param(clockdelay_nt, uint, S_IRUGO | S_IWUSR);
+module_param_array(type, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(pcm, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(dslot, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(iomode, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(port, uint, NULL, S_IRUGO | S_IWUSR);
+
+#ifdef HFC_REGISTER_DEBUG
+#define HFC_outb(hc, reg, val) \
+	(hc->HFC_outb(hc, reg, val, __func__, __LINE__))
+#define HFC_outb_nodebug(hc, reg, val) \
+	(hc->HFC_outb_nodebug(hc, reg, val, __func__, __LINE__))
+#define HFC_inb(hc, reg) \
+	(hc->HFC_inb(hc, reg, __func__, __LINE__))
+#define HFC_inb_nodebug(hc, reg) \
+	(hc->HFC_inb_nodebug(hc, reg, __func__, __LINE__))
+#define HFC_inw(hc, reg) \
+	(hc->HFC_inw(hc, reg, __func__, __LINE__))
+#define HFC_inw_nodebug(hc, reg) \
+	(hc->HFC_inw_nodebug(hc, reg, __func__, __LINE__))
+#define HFC_wait(hc) \
+	(hc->HFC_wait(hc, __func__, __LINE__))
+#define HFC_wait_nodebug(hc) \
+	(hc->HFC_wait_nodebug(hc, __func__, __LINE__))
+#else
+#define HFC_outb(hc, reg, val)		(hc->HFC_outb(hc, reg, val))
+#define HFC_outb_nodebug(hc, reg, val)	(hc->HFC_outb_nodebug(hc, reg, val))
+#define HFC_inb(hc, reg)		(hc->HFC_inb(hc, reg))
+#define HFC_inb_nodebug(hc, reg)	(hc->HFC_inb_nodebug(hc, reg))
+#define HFC_inw(hc, reg)		(hc->HFC_inw(hc, reg))
+#define HFC_inw_nodebug(hc, reg)	(hc->HFC_inw_nodebug(hc, reg))
+#define HFC_wait(hc)			(hc->HFC_wait(hc))
+#define HFC_wait_nodebug(hc)		(hc->HFC_wait_nodebug(hc))
+#endif
+
+/* HFC_IO_MODE_PCIMEM */
+static void
+#ifdef HFC_REGISTER_DEBUG
+HFC_outb_pcimem(struct hfc_multi *hc, u_char reg, u_char val,
+		const char *function, int line)
+#else
+HFC_outb_pcimem(struct hfc_multi *hc, u_char reg, u_char val)
+#endif
+{
+	writeb(val, (hc->pci_membase)+reg);
+}
+static u_char
+#ifdef HFC_REGISTER_DEBUG
+HFC_inb_pcimem(struct hfc_multi *hc, u_char reg, const char *function, int line)
+#else
+HFC_inb_pcimem(struct hfc_multi *hc, u_char reg)
+#endif
+{
+	return readb((hc->pci_membase)+reg);
+}
+static u_short
+#ifdef HFC_REGISTER_DEBUG
+HFC_inw_pcimem(struct hfc_multi *hc, u_char reg, const char *function, int line)
+#else
+HFC_inw_pcimem(struct hfc_multi *hc, u_char reg)
+#endif
+{
+	return readw((hc->pci_membase)+reg);
+}
+static void
+#ifdef HFC_REGISTER_DEBUG
+HFC_wait_pcimem(struct hfc_multi *hc, const char *function, int line)
+#else
+HFC_wait_pcimem(struct hfc_multi *hc)
+#endif
+{
+	while (readb((hc->pci_membase)+R_STATUS) & V_BUSY);
+}
+
+/* HFC_IO_MODE_REGIO */
+static void
+#ifdef HFC_REGISTER_DEBUG
+HFC_outb_regio(struct hfc_multi *hc, u_char reg, u_char val,
+	const char *function, int line)
+#else
+HFC_outb_regio(struct hfc_multi *hc, u_char reg, u_char val)
+#endif
+{
+	outb(reg, (hc->pci_iobase)+4);
+	outb(val, hc->pci_iobase);
+}
+static u_char
+#ifdef HFC_REGISTER_DEBUG
+HFC_inb_regio(struct hfc_multi *hc, u_char reg, const char *function, int line)
+#else
+HFC_inb_regio(struct hfc_multi *hc, u_char reg)
+#endif
+{
+	outb(reg, (hc->pci_iobase)+4);
+	return inb(hc->pci_iobase);
+}
+static u_short
+#ifdef HFC_REGISTER_DEBUG
+HFC_inw_regio(struct hfc_multi *hc, u_char reg, const char *function, int line)
+#else
+HFC_inw_regio(struct hfc_multi *hc, u_char reg)
+#endif
+{
+	outb(reg, (hc->pci_iobase)+4);
+	return inw(hc->pci_iobase);
+}
+static void
+#ifdef HFC_REGISTER_DEBUG
+HFC_wait_regio(struct hfc_multi *hc, const char *function, int line)
+#else
+HFC_wait_regio(struct hfc_multi *hc)
+#endif
+{
+	outb(R_STATUS, (hc->pci_iobase)+4);
+	while (inb(hc->pci_iobase) & V_BUSY);
+}
+
+#ifdef HFC_REGISTER_DEBUG
+static void
+HFC_outb_debug(struct hfc_multi *hc, u_char reg, u_char val,
+		const char *function, int line)
+{
+	char regname[256] = "", bits[9] = "xxxxxxxx";
+	int i;
+
+	i = -1;
+	while (hfc_register_names[++i].name) {
+		if (hfc_register_names[i].reg == reg)
+			strcat(regname, hfc_register_names[i].name);
+	}
+	if (regname[0] == '\0')
+		strcpy(regname, "register");
+
+	bits[7] = '0'+(!!(val&1));
+	bits[6] = '0'+(!!(val&2));
+	bits[5] = '0'+(!!(val&4));
+	bits[4] = '0'+(!!(val&8));
+	bits[3] = '0'+(!!(val&16));
+	bits[2] = '0'+(!!(val&32));
+	bits[1] = '0'+(!!(val&64));
+	bits[0] = '0'+(!!(val&128));
+	printk(KERN_DEBUG
+	    "HFC_outb(chip %d, %02x=%s, 0x%02x=%s); in %s() line %d\n",
+	    hc->id, reg, regname, val, bits, function, line);
+	HFC_outb_nodebug(hc, reg, val);
+}
+static u_char
+HFC_inb_debug(struct hfc_multi *hc, u_char reg, const char *function, int line)
+{
+	char regname[256] = "", bits[9] = "xxxxxxxx";
+	u_char val = HFC_inb_nodebug(hc, reg);
+	int i;
+
+	i = 0;
+	while (hfc_register_names[i++].name)
+		;
+	while (hfc_register_names[++i].name) {
+		if (hfc_register_names[i].reg == reg)
+			strcat(regname, hfc_register_names[i].name);
+	}
+	if (regname[0] == '\0')
+		strcpy(regname, "register");
+
+	bits[7] = '0'+(!!(val&1));
+	bits[6] = '0'+(!!(val&2));
+	bits[5] = '0'+(!!(val&4));
+	bits[4] = '0'+(!!(val&8));
+	bits[3] = '0'+(!!(val&16));
+	bits[2] = '0'+(!!(val&32));
+	bits[1] = '0'+(!!(val&64));
+	bits[0] = '0'+(!!(val&128));
+	printk(KERN_DEBUG
+	    "HFC_inb(chip %d, %02x=%s) = 0x%02x=%s; in %s() line %d\n",
+	    hc->id, reg, regname, val, bits, function, line);
+	return val;
+}
+static u_short
+HFC_inw_debug(struct hfc_multi *hc, u_char reg, const char *function, int line)
+{
+	char regname[256] = "";
+	u_short val = HFC_inw_nodebug(hc, reg);
+	int i;
+
+	i = 0;
+	while (hfc_register_names[i++].name)
+		;
+	while (hfc_register_names[++i].name) {
+		if (hfc_register_names[i].reg == reg)
+			strcat(regname, hfc_register_names[i].name);
+	}
+	if (regname[0] == '\0')
+		strcpy(regname, "register");
+
+	printk(KERN_DEBUG
+	    "HFC_inw(chip %d, %02x=%s) = 0x%04x; in %s() line %d\n",
+	    hc->id, reg, regname, val, function, line);
+	return val;
+}
+static void
+HFC_wait_debug(struct hfc_multi *hc, const char *function, int line)
+{
+	printk(KERN_DEBUG "HFC_wait(chip %d); in %s() line %d\n",
+	    hc->id, function, line);
+	HFC_wait_nodebug(hc);
+}
+#endif
+
+/* write fifo data (REGIO) */
+void
+write_fifo_regio(struct hfc_multi *hc, u_char *data, int len)
+{
+	outb(A_FIFO_DATA0, (hc->pci_iobase)+4);
+	while (len>>2) {
+		outl(*(u32 *)data, hc->pci_iobase);
+		data += 4;
+		len -= 4;
+	}
+	while (len>>1) {
+		outw(*(u16 *)data, hc->pci_iobase);
+		data += 2;
+		len -= 2;
+	}
+	while (len) {
+		outb(*data, hc->pci_iobase);
+		data++;
+		len--;
+	}
+}
+/* write fifo data (PCIMEM) */
+void
+write_fifo_pcimem(struct hfc_multi *hc, u_char *data, int len)
+{
+	while (len>>2) {
+		writel(*(u32 *)data, (hc->pci_membase)+A_FIFO_DATA0);
+		data += 4;
+		len -= 4;
+	}
+	while (len>>1) {
+		writew(*(u16 *)data, (hc->pci_membase)+A_FIFO_DATA0);
+		data += 2;
+		len -= 2;
+	}
+	while (len) {
+		writeb(*data, (hc->pci_membase)+A_FIFO_DATA0);
+		data++;
+		len--;
+	}
+}
+/* read fifo data (REGIO) */
+void
+read_fifo_regio(struct hfc_multi *hc, u_char *data, int len)
+{
+	outb(A_FIFO_DATA0, (hc->pci_iobase)+4);
+	while (len>>2) {
+		*(u32 *)data = inl(hc->pci_iobase);
+		data += 4;
+		len -= 4;
+	}
+	while (len>>1) {
+		*(u16 *)data = inw(hc->pci_iobase);
+		data += 2;
+		len -= 2;
+	}
+	while (len) {
+		*data = inb(hc->pci_iobase);
+		data++;
+		len--;
+	}
+}
+
+/* read fifo data (PCIMEM) */
+void
+read_fifo_pcimem(struct hfc_multi *hc, u_char *data, int len)
+{
+	while (len>>2) {
+		*(u32 *)data =
+			readl((hc->pci_membase)+A_FIFO_DATA0);
+		data += 4;
+		len -= 4;
+	}
+	while (len>>1) {
+		*(u16 *)data =
+			readw((hc->pci_membase)+A_FIFO_DATA0);
+		data += 2;
+		len -= 2;
+	}
+	while (len) {
+		*data = readb((hc->pci_membase)+A_FIFO_DATA0);
+		data++;
+		len--;
+	}
+}
+
+
+static void
+enable_hwirq(struct hfc_multi *hc)
+{
+	hc->hw.r_irq_ctrl |= V_GLOB_IRQ_EN;
+	HFC_outb(hc, R_IRQ_CTRL, hc->hw.r_irq_ctrl);
+}
+
+static void
+disable_hwirq(struct hfc_multi *hc)
+{
+	hc->hw.r_irq_ctrl &= ~((u_char)V_GLOB_IRQ_EN);
+	HFC_outb(hc, R_IRQ_CTRL, hc->hw.r_irq_ctrl);
+}
+
+#define	NUM_EC 2
+#define	MAX_TDM_CHAN 32
+
+
+inline void
+enablepcibridge(struct hfc_multi *c)
+{
+	HFC_outb(c, R_BRG_PCM_CFG, (0x0 << 6) | 0x3); /* was _io before */
+}
+
+inline void
+disablepcibridge(struct hfc_multi *c)
+{
+	HFC_outb(c, R_BRG_PCM_CFG, (0x0 << 6) | 0x2); /* was _io before */
+}
+
+inline unsigned char
+readpcibridge(struct hfc_multi *hc, unsigned char address)
+{
+	unsigned short cipv;
+	unsigned char data;
+
+	if (!hc->pci_iobase)
+		return 0;
+
+	/* slow down a PCI read access by 1 PCI clock cycle */
+	HFC_outb(hc, R_CTRL, 0x4); /*was _io before*/
+
+	if (address == 0)
+		cipv = 0x4000;
+	else
+		cipv = 0x5800;
+
+	/* select local bridge port address by writing to CIP port */
+	/* data = HFC_inb(c, cipv); * was _io before */
+	outw(cipv, hc->pci_iobase + 4);
+	data = inb(hc->pci_iobase);
+
+	/* restore R_CTRL for normal PCI read cycle speed */
+	HFC_outb(hc, R_CTRL, 0x0); /* was _io before */
+
+	return data;
+}
+
+inline void
+writepcibridge(struct hfc_multi *hc, unsigned char address, unsigned char data)
+{
+	unsigned short cipv;
+	unsigned int datav;
+
+	if (!hc->pci_iobase)
+		return;
+
+	if (address == 0)
+		cipv = 0x4000;
+	else
+		cipv = 0x5800;
+
+	/* select local bridge port address by writing to CIP port */
+	outw(cipv, hc->pci_iobase + 4);
+	/* define a 32 bit dword with 4 identical bytes for write sequence */
+	datav = data | ((__u32) data << 8) | ((__u32) data << 16) |
+	    ((__u32) data << 24);
+
+	/*
+	 * write this 32 bit dword to the bridge data port
+	 * this will initiate a write sequence of up to 4 writes to the same
+	 * address on the local bus interface the number of write accesses
+	 * is undefined but >=1 and depends on the next PCI transaction
+	 * during write sequence on the local bus
+	 */
+	outl(datav, hc->pci_iobase);
+}
+
+inline void
+cpld_set_reg(struct hfc_multi *hc, unsigned char reg)
+{
+	/* Do data pin read low byte */
+	HFC_outb(hc, R_GPIO_OUT1, reg);
+}
+
+inline void
+cpld_write_reg(struct hfc_multi *hc, unsigned char reg, unsigned char val)
+{
+	cpld_set_reg(hc, reg);
+
+	enablepcibridge(hc);
+	writepcibridge(hc, 1, val);
+	disablepcibridge(hc);
+
+	return;
+}
+
+inline unsigned char
+cpld_read_reg(struct hfc_multi *hc, unsigned char reg)
+{
+	unsigned char bytein;
+
+	cpld_set_reg(hc, reg);
+
+	/* Do data pin read low byte */
+	HFC_outb(hc, R_GPIO_OUT1, reg);
+
+	enablepcibridge(hc);
+	bytein = readpcibridge(hc, 1);
+	disablepcibridge(hc);
+
+	return bytein;
+}
+
+inline void
+vpm_write_address(struct hfc_multi *hc, unsigned short addr)
+{
+	cpld_write_reg(hc, 0, 0xff & addr);
+	cpld_write_reg(hc, 1, 0x01 & (addr >> 8));
+}
+
+inline unsigned short
+vpm_read_address(struct hfc_multi *c)
+{
+	unsigned short addr;
+	unsigned short highbit;
+
+	addr = cpld_read_reg(c, 0);
+	highbit = cpld_read_reg(c, 1);
+
+	addr = addr | (highbit << 8);
+
+	return addr & 0x1ff;
+}
+
+inline unsigned char
+vpm_in(struct hfc_multi *c, int which, unsigned short addr)
+{
+	unsigned char res;
+
+	vpm_write_address(c, addr);
+
+	if (!which)
+		cpld_set_reg(c, 2);
+	else
+		cpld_set_reg(c, 3);
+
+	enablepcibridge(c);
+	res = readpcibridge(c, 1);
+	disablepcibridge(c);
+
+	cpld_set_reg(c, 0);
+
+	return res;
+}
+
+inline void
+vpm_out(struct hfc_multi *c, int which, unsigned short addr,
+    unsigned char data)
+{
+	vpm_write_address(c, addr);
+
+	enablepcibridge(c);
+
+	if (!which)
+		cpld_set_reg(c, 2);
+	else
+		cpld_set_reg(c, 3);
+
+	writepcibridge(c, 1, data);
+
+	cpld_set_reg(c, 0);
+
+	disablepcibridge(c);
+
+	{
+	unsigned char regin;
+	regin = vpm_in(c, which, addr);
+	if (regin != data)
+		printk(KERN_DEBUG "Wrote 0x%x to register 0x%x but got back "
+			"0x%x\n", data, addr, regin);
+	}
+
+}
+
+
+void
+vpm_init(struct hfc_multi *wc)
+{
+	unsigned char reg;
+	unsigned int mask;
+	unsigned int i, x, y;
+	unsigned int ver;
+
+	for (x = 0; x < NUM_EC; x++) {
+		/* Setup GPIO's */
+		if (!x) {
+			ver = vpm_in(wc, x, 0x1a0);
+			printk(KERN_DEBUG "VPM: Chip %d: ver %02x\n", x, ver);
+		}
+
+		for (y = 0; y < 4; y++) {
+			vpm_out(wc, x, 0x1a8 + y, 0x00); /* GPIO out */
+			vpm_out(wc, x, 0x1ac + y, 0x00); /* GPIO dir */
+			vpm_out(wc, x, 0x1b0 + y, 0x00); /* GPIO sel */
+		}
+
+		/* Setup TDM path - sets fsync and tdm_clk as inputs */
+		reg = vpm_in(wc, x, 0x1a3); /* misc_con */
+		vpm_out(wc, x, 0x1a3, reg & ~2);
+
+		/* Setup Echo length (256 taps) */
+		vpm_out(wc, x, 0x022, 1);
+		vpm_out(wc, x, 0x023, 0xff);
+
+		/* Setup timeslots */
+		vpm_out(wc, x, 0x02f, 0x00);
+		mask = 0x02020202 << (x * 4);
+
+		/* Setup the tdm channel masks for all chips */
+		for (i = 0; i < 4; i++)
+			vpm_out(wc, x, 0x33 - i, (mask >> (i << 3)) & 0xff);
+
+		/* Setup convergence rate */
+		printk(KERN_DEBUG "VPM: A-law mode\n");
+		reg = 0x00 | 0x10 | 0x01;
+		vpm_out(wc, x, 0x20, reg);
+		printk(KERN_DEBUG "VPM reg 0x20 is %x\n", reg);
+		/*vpm_out(wc, x, 0x20, (0x00 | 0x08 | 0x20 | 0x10)); */
+
+		vpm_out(wc, x, 0x24, 0x02);
+		reg = vpm_in(wc, x, 0x24);
+		printk(KERN_DEBUG "NLP Thresh is set to %d (0x%x)\n", reg, reg);
+
+		/* Initialize echo cans */
+		for (i = 0; i < MAX_TDM_CHAN; i++) {
+			if (mask & (0x00000001 << i))
+				vpm_out(wc, x, i, 0x00);
+		}
+
+		/*
+		 * ARM arch at least disallows a udelay of
+		 * more than 2ms... it gives a fake "__bad_udelay"
+		 * reference at link-time.
+		 * long delays in kernel code are pretty sucky anyway
+		 * for now work around it using 5 x 2ms instead of 1 x 10ms
+		 */
+
+		udelay(2000);
+		udelay(2000);
+		udelay(2000);
+		udelay(2000);
+		udelay(2000);
+
+		/* Put in bypass mode */
+		for (i = 0; i < MAX_TDM_CHAN; i++) {
+			if (mask & (0x00000001 << i))
+				vpm_out(wc, x, i, 0x01);
+		}
+
+		/* Enable bypass */
+		for (i = 0; i < MAX_TDM_CHAN; i++) {
+			if (mask & (0x00000001 << i))
+				vpm_out(wc, x, 0x78 + i, 0x01);
+		}
+
+	}
+}
+
+void
+vpm_check(struct hfc_multi *hctmp)
+{
+	unsigned char gpi2;
+
+	gpi2 = HFC_inb(hctmp, R_GPI_IN2);
+
+	if ((gpi2 & 0x3) != 0x3)
+		printk(KERN_DEBUG "Got interrupt 0x%x from VPM!\n", gpi2);
+}
+
+
+/*
+ * Interface to enable/disable the HW Echocan
+ *
+ * these functions are called within a spin_lock_irqsave on
+ * the channel instance lock, so we are not disturbed by irqs
+ *
+ * we can later easily change the interface to make  other
+ * things configurable, for now we configure the taps
+ *
+ */
+
+void
+vpm_echocan_on(struct hfc_multi *hc, int ch, int taps)
+{
+	unsigned int timeslot;
+	unsigned int unit;
+	struct bchannel *bch = hc->chan[ch].bch;
+#ifdef TXADJ
+	int txadj = -4;
+	struct sk_buff *skb;
+#endif
+	if (hc->chan[ch].protocol != ISDN_P_B_RAW)
+		return;
+
+	if (!bch)
+		return;
+
+#ifdef TXADJ
+	skb = _alloc_mISDN_skb(PH_CONTROL_IND, HFC_VOL_CHANGE_TX,
+		sizeof(int), &txadj, GFP_ATOMIC);
+	if (skb)
+		recv_Bchannel_skb(bch, skb);
+#endif
+
+	timeslot = ((ch/4)*8) + ((ch%4)*4) + 1;
+	unit = ch % 4;
+
+	printk(KERN_NOTICE "vpm_echocan_on called taps [%d] on timeslot %d\n",
+	    taps, timeslot);
+
+	vpm_out(hc, unit, timeslot, 0x7e);
+}
+
+void
+vpm_echocan_off(struct hfc_multi *hc, int ch)
+{
+	unsigned int timeslot;
+	unsigned int unit;
+	struct bchannel *bch = hc->chan[ch].bch;
+#ifdef TXADJ
+	int txadj = 0;
+	struct sk_buff *skb;
+#endif
+
+	if (hc->chan[ch].protocol != ISDN_P_B_RAW)
+		return;
+
+	if (!bch)
+		return;
+
+#ifdef TXADJ
+	skb = _alloc_mISDN_skb(PH_CONTROL_IND, HFC_VOL_CHANGE_TX,
+		sizeof(int), &txadj, GFP_ATOMIC);
+	if (skb)
+		recv_Bchannel_skb(bch, skb);
+#endif
+
+	timeslot = ((ch/4)*8) + ((ch%4)*4) + 1;
+	unit = ch % 4;
+
+	printk(KERN_NOTICE "vpm_echocan_off called on timeslot %d\n",
+	    timeslot);
+	/* FILLME */
+	vpm_out(hc, unit, timeslot, 0x01);
+}
+
+
+/*
+ * Speech Design resync feature
+ * NOTE: This is called sometimes outside interrupt handler.
+ * We must lock irqsave, so no other interrupt (other card) will occurr!
+ * Also multiple interrupts may nest, so must lock each access (lists, card)!
+ */
+static inline void
+hfcmulti_resync(struct hfc_multi *locked, struct hfc_multi *newmaster, int rm)
+{
+	struct hfc_multi *hc, *next, *pcmmaster = 0;
+	u_int *plx_acc_32, pv;
+	u_long flags;
+
+	spin_lock_irqsave(&HFClock, flags);
+	spin_lock(&plx_lock); /* must be locked inside other locks */
+
+	if (debug & DEBUG_HFCMULTI_PLXSD)
+		printk(KERN_DEBUG "%s: RESYNC(syncmaster=0x%p)\n",
+			__func__, syncmaster);
+
+	/* select new master */
+	if (newmaster) {
+		if (debug & DEBUG_HFCMULTI_PLXSD)
+			printk(KERN_DEBUG "using provided controller\n");
+	} else {
+		list_for_each_entry_safe(hc, next, &HFClist, list) {
+			if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+				if (hc->syncronized) {
+					newmaster = hc;
+					break;
+				}
+			}
+		}
+	}
+
+	/* Disable sync of all cards */
+	list_for_each_entry_safe(hc, next, &HFClist, list) {
+		if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+			pv = readl(plx_acc_32);
+			pv &= ~PLX_SYNC_O_EN;
+			writel(pv, plx_acc_32);
+			if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip)) {
+				pcmmaster = hc;
+				if (hc->type == 1) {
+					if (debug & DEBUG_HFCMULTI_PLXSD)
+						printk(KERN_DEBUG
+							"Schedule SYNC_I\n");
+					hc->e1_resync |= 1; /* get SYNC_I */
+				}
+			}
+		}
+	}
+
+	if (newmaster) {
+		hc = newmaster;
+		if (debug & DEBUG_HFCMULTI_PLXSD)
+			printk(KERN_DEBUG "id=%d (0x%p) = syncronized with "
+				"interface.\n", hc->id, hc);
+		/* Enable new sync master */
+		plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+		pv = readl(plx_acc_32);
+		pv |= PLX_SYNC_O_EN;
+		writel(pv, plx_acc_32);
+		/* switch to jatt PLL, if not disabled by RX_SYNC */
+		if (hc->type == 1 && !test_bit(HFC_CHIP_RX_SYNC, &hc->chip)) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG "Schedule jatt PLL\n");
+			hc->e1_resync |= 2; /* switch to jatt */
+		}
+	} else {
+		if (pcmmaster) {
+			hc = pcmmaster;
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG
+					"id=%d (0x%p) = PCM master syncronized "
+					"with QUARTZ\n", hc->id, hc);
+			if (hc->type == 1) {
+				/* Use the crystal clock for the PCM
+				   master card */
+				if (debug & DEBUG_HFCMULTI_PLXSD)
+					printk(KERN_DEBUG
+					    "Schedule QUARTZ for HFC-E1\n");
+				hc->e1_resync |= 4; /* switch quartz */
+			} else {
+				if (debug & DEBUG_HFCMULTI_PLXSD)
+					printk(KERN_DEBUG
+					    "QUARTZ is automatically "
+					    "enabled by HFC-%dS\n", hc->type);
+			}
+			plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+			pv = readl(plx_acc_32);
+			pv |= PLX_SYNC_O_EN;
+			writel(pv, plx_acc_32);
+		} else
+			if (!rm)
+				printk(KERN_ERR "%s no pcm master, this MUST "
+					"not happen!\n", __func__);
+	}
+	syncmaster = newmaster;
+
+	spin_unlock(&plx_lock);
+	spin_unlock_irqrestore(&HFClock, flags);
+}
+
+/* This must be called AND hc must be locked irqsave!!! */
+inline void
+plxsd_checksync(struct hfc_multi *hc, int rm)
+{
+	if (hc->syncronized) {
+		if (syncmaster == NULL) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_WARNING "%s: GOT sync on card %d"
+					" (id=%d)\n", __func__, hc->id + 1,
+					hc->id);
+			hfcmulti_resync(hc, hc, rm);
+		}
+	} else {
+		if (syncmaster == hc) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_WARNING "%s: LOST sync on card %d"
+					" (id=%d)\n", __func__, hc->id + 1,
+					hc->id);
+			hfcmulti_resync(hc, NULL, rm);
+		}
+	}
+}
+
+
+/*
+ * free hardware resources used by driver
+ */
+static void
+release_io_hfcmulti(struct hfc_multi *hc)
+{
+	u_int	*plx_acc_32, pv;
+	u_long	plx_flags;
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: entered\n", __func__);
+
+	/* soft reset also masks all interrupts */
+	hc->hw.r_cirm |= V_SRES;
+	HFC_outb(hc, R_CIRM, hc->hw.r_cirm);
+	udelay(1000);
+	hc->hw.r_cirm &= ~V_SRES;
+	HFC_outb(hc, R_CIRM, hc->hw.r_cirm);
+	udelay(1000); /* instead of 'wait' that may cause locking */
+
+	/* release Speech Design card, if PLX was initialized */
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip) && hc->plx_membase) {
+		if (debug & DEBUG_HFCMULTI_PLXSD)
+			printk(KERN_DEBUG "%s: release PLXSD card %d\n",
+			    __func__, hc->id + 1);
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+		writel(PLX_GPIOC_INIT, plx_acc_32);
+		pv = readl(plx_acc_32);
+		/* Termination off */
+		pv &= ~PLX_TERM_ON;
+		/* Disconnect the PCM */
+		pv |= PLX_SLAVE_EN_N;
+		pv &= ~PLX_MASTER_EN;
+		pv &= ~PLX_SYNC_O_EN;
+		/* Put the DSP in Reset */
+		pv &= ~PLX_DSP_RES_N;
+		writel(pv, plx_acc_32);
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_WARNING "%s: PCM off: PLX_GPIO=%x\n",
+				__func__, pv);
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+	}
+
+	/* disable memory mapped ports / io ports */
+	test_and_clear_bit(HFC_CHIP_PLXSD, &hc->chip); /* prevent resync */
+	pci_write_config_word(hc->pci_dev, PCI_COMMAND, 0);
+	if (hc->pci_membase)
+		iounmap((void *)hc->pci_membase);
+	if (hc->plx_membase)
+		iounmap((void *)hc->plx_membase);
+	if (hc->pci_iobase)
+		release_region(hc->pci_iobase, 8);
+
+	if (hc->pci_dev) {
+		pci_disable_device(hc->pci_dev);
+		pci_set_drvdata(hc->pci_dev, NULL);
+	}
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: done\n", __func__);
+}
+
+/*
+ * function called to reset the HFC chip. A complete software reset of chip
+ * and fifos is done. All configuration of the chip is done.
+ */
+
+static int
+init_chip(struct hfc_multi *hc)
+{
+	u_long			flags, val, val2 = 0, rev;
+	int			i, err = 0;
+	u_char			r_conf_en, rval;
+	u_int			*plx_acc_32, pv;
+	u_long			plx_flags, hfc_flags;
+	int			plx_count;
+	struct hfc_multi	*pos, *next, *plx_last_hc;
+
+	spin_lock_irqsave(&hc->lock, flags);
+	/* reset all registers */
+	memset(&hc->hw, 0, sizeof(struct hfcm_hw));
+
+	/* revision check */
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: entered\n", __func__);
+	val = HFC_inb(hc, R_CHIP_ID)>>4;
+	if (val != 0x8 && val != 0xc && val != 0xe) {
+		printk(KERN_INFO "HFC_multi: unknown CHIP_ID:%x\n", (u_int)val);
+		err = -EIO;
+		goto out;
+	}
+	rev = HFC_inb(hc, R_CHIP_RV);
+	printk(KERN_INFO
+	    "HFC_multi: detected HFC with chip ID=0x%lx revision=%ld%s\n",
+	    val, rev, (rev == 0) ? " (old FIFO handling)" : "");
+	if (rev == 0) {
+		test_and_set_bit(HFC_CHIP_REVISION0, &hc->chip);
+		printk(KERN_WARNING
+		    "HFC_multi: NOTE: Your chip is revision 0, "
+		    "ask Cologne Chip for update. Newer chips "
+		    "have a better FIFO handling. Old chips "
+		    "still work but may have slightly lower "
+		    "HDLC transmit performance.\n");
+	}
+	if (rev > 1) {
+		printk(KERN_WARNING "HFC_multi: WARNING: This driver doesn't "
+		    "consider chip revision = %ld. The chip / "
+		    "bridge may not work.\n", rev);
+	}
+
+	/* set s-ram size */
+	hc->Flen = 0x10;
+	hc->Zmin = 0x80;
+	hc->Zlen = 384;
+	hc->DTMFbase = 0x1000;
+	if (test_bit(HFC_CHIP_EXRAM_128, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: changing to 128K extenal RAM\n",
+			    __func__);
+		hc->hw.r_ctrl |= V_EXT_RAM;
+		hc->hw.r_ram_sz = 1;
+		hc->Flen = 0x20;
+		hc->Zmin = 0xc0;
+		hc->Zlen = 1856;
+		hc->DTMFbase = 0x2000;
+	}
+	if (test_bit(HFC_CHIP_EXRAM_512, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: changing to 512K extenal RAM\n",
+			    __func__);
+		hc->hw.r_ctrl |= V_EXT_RAM;
+		hc->hw.r_ram_sz = 2;
+		hc->Flen = 0x20;
+		hc->Zmin = 0xc0;
+		hc->Zlen = 8000;
+		hc->DTMFbase = 0x2000;
+	}
+	hc->max_trans = poll << 1;
+	if (hc->max_trans > hc->Zlen)
+		hc->max_trans = hc->Zlen;
+
+	/* Speech Design PLX bridge */
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_PLXSD)
+			printk(KERN_DEBUG "%s: initializing PLXSD card %d\n",
+			    __func__, hc->id + 1);
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+		writel(PLX_GPIOC_INIT, plx_acc_32);
+		pv = readl(plx_acc_32);
+		/* The first and the last cards are terminating the PCM bus */
+		pv |= PLX_TERM_ON; /* hc is currently the last */
+		/* Disconnect the PCM */
+		pv |= PLX_SLAVE_EN_N;
+		pv &= ~PLX_MASTER_EN;
+		pv &= ~PLX_SYNC_O_EN;
+		/* Put the DSP in Reset */
+		pv &= ~PLX_DSP_RES_N;
+		writel(pv, plx_acc_32);
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_WARNING "%s: slave/term: PLX_GPIO=%x\n",
+				__func__, pv);
+		/*
+		 * If we are the 3rd PLXSD card or higher, we must turn
+		 * termination of last PLXSD card off.
+		 */
+		spin_lock_irqsave(&HFClock, hfc_flags);
+		plx_count = 0;
+		plx_last_hc = NULL;
+		list_for_each_entry_safe(pos, next, &HFClist, list) {
+			if (test_bit(HFC_CHIP_PLXSD, &pos->chip)) {
+				plx_count++;
+				if (pos != hc)
+					plx_last_hc = pos;
+			}
+		}
+		if (plx_count >= 3) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG "%s: card %d is between, so "
+					"we disable termination\n",
+				    __func__, plx_last_hc->id + 1);
+			spin_lock_irqsave(&plx_lock, plx_flags);
+			plx_acc_32 = (u_int *)(plx_last_hc->plx_membase
+					+ PLX_GPIOC);
+			pv = readl(plx_acc_32);
+			pv &= ~PLX_TERM_ON;
+			writel(pv, plx_acc_32);
+			spin_unlock_irqrestore(&plx_lock, plx_flags);
+			if (debug & DEBUG_HFCMULTI_INIT)
+			    printk(KERN_WARNING "%s: term off: PLX_GPIO=%x\n",
+					__func__, pv);
+		}
+		spin_unlock_irqrestore(&HFClock, hfc_flags);
+		hc->hw.r_pcm_md0 = V_F0_LEN; /* shift clock for DSP */
+	}
+
+	/* we only want the real Z2 read-pointer for revision > 0 */
+	if (!test_bit(HFC_CHIP_REVISION0, &hc->chip))
+		hc->hw.r_ram_sz |= V_FZ_MD;
+
+	/* select pcm mode */
+	if (test_bit(HFC_CHIP_PCM_SLAVE, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: setting PCM into slave mode\n",
+			    __func__);
+	} else
+	if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip) && !plxsd_master) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: setting PCM into master mode\n",
+			    __func__);
+		hc->hw.r_pcm_md0 |= V_PCM_MD;
+	} else {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: performing PCM auto detect\n",
+			    __func__);
+	}
+
+	/* soft reset */
+	HFC_outb(hc, R_CTRL, hc->hw.r_ctrl);
+	HFC_outb(hc, R_RAM_SZ, hc->hw.r_ram_sz);
+	HFC_outb(hc, R_FIFO_MD, 0);
+	hc->hw.r_cirm = V_SRES | V_HFCRES | V_PCMRES | V_STRES | V_RLD_EPR;
+	HFC_outb(hc, R_CIRM, hc->hw.r_cirm);
+	udelay(100);
+	hc->hw.r_cirm = 0;
+	HFC_outb(hc, R_CIRM, hc->hw.r_cirm);
+	udelay(100);
+	HFC_outb(hc, R_RAM_SZ, hc->hw.r_ram_sz);
+
+	/* Speech Design PLX bridge pcm and sync mode */
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+		pv = readl(plx_acc_32);
+		/* Connect PCM */
+		if (hc->hw.r_pcm_md0 & V_PCM_MD) {
+			pv |= PLX_MASTER_EN | PLX_SLAVE_EN_N;
+			pv |= PLX_SYNC_O_EN;
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_WARNING "%s: master: PLX_GPIO=%x\n",
+					__func__, pv);
+		} else {
+			pv &= ~(PLX_MASTER_EN | PLX_SLAVE_EN_N);
+			pv &= ~PLX_SYNC_O_EN;
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_WARNING "%s: slave: PLX_GPIO=%x\n",
+					__func__, pv);
+		}
+		writel(pv, plx_acc_32);
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+	}
+
+	/* PCM setup */
+	HFC_outb(hc, R_PCM_MD0, hc->hw.r_pcm_md0 | 0x90);
+	if (hc->slots == 32)
+		HFC_outb(hc, R_PCM_MD1, 0x00);
+	if (hc->slots == 64)
+		HFC_outb(hc, R_PCM_MD1, 0x10);
+	if (hc->slots == 128)
+		HFC_outb(hc, R_PCM_MD1, 0x20);
+	HFC_outb(hc, R_PCM_MD0, hc->hw.r_pcm_md0 | 0xa0);
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip))
+		HFC_outb(hc, R_PCM_MD2, V_SYNC_SRC); /* sync via SYNC_I / O */
+	else
+		HFC_outb(hc, R_PCM_MD2, 0x00); /* sync from interface */
+	HFC_outb(hc, R_PCM_MD0, hc->hw.r_pcm_md0 | 0x00);
+	for (i = 0; i < 256; i++) {
+		HFC_outb_nodebug(hc, R_SLOT, i);
+		HFC_outb_nodebug(hc, A_SL_CFG, 0);
+		HFC_outb_nodebug(hc, A_CONF, 0);
+		hc->slot_owner[i] = -1;
+	}
+
+	/* set clock speed */
+	if (test_bit(HFC_CHIP_CLOCK2, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: setting double clock\n", __func__);
+		HFC_outb(hc, R_BRG_PCM_CFG, V_PCM_CLK);
+	}
+
+	/* B410P GPIO */
+	if (test_bit(HFC_CHIP_B410P, &hc->chip)) {
+		printk(KERN_NOTICE "Setting GPIOs\n");
+		HFC_outb(hc, R_GPIO_SEL, 0x30);
+		HFC_outb(hc, R_GPIO_EN1, 0x3);
+		udelay(1000);
+		printk(KERN_NOTICE "calling vpm_init\n");
+		vpm_init(hc);
+	}
+
+	/* check if R_F0_CNT counts (8 kHz frame count) */
+	val = HFC_inb(hc, R_F0_CNTL);
+	val += HFC_inb(hc, R_F0_CNTH) << 8;
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG
+		    "HFC_multi F0_CNT %ld after reset\n", val);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	set_current_state(TASK_UNINTERRUPTIBLE);
+	schedule_timeout((HZ/100)?:1); /* Timeout minimum 10ms */
+	spin_lock_irqsave(&hc->lock, flags);
+	val2 = HFC_inb(hc, R_F0_CNTL);
+	val2 += HFC_inb(hc, R_F0_CNTH) << 8;
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG
+			"HFC_multi F0_CNT %ld after 10 ms (1st try)\n",
+		    val2);
+	if (val2 >= val+8) { /* 1 ms */
+		/* it counts, so we keep the pcm mode */
+		if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip))
+			printk(KERN_INFO "controller is PCM bus MASTER\n");
+		else
+		if (test_bit(HFC_CHIP_PCM_SLAVE, &hc->chip))
+			printk(KERN_INFO "controller is PCM bus SLAVE\n");
+		else {
+			test_and_set_bit(HFC_CHIP_PCM_SLAVE, &hc->chip);
+			printk(KERN_INFO "controller is PCM bus SLAVE "
+				"(auto detected)\n");
+		}
+	} else {
+		/* does not count */
+		if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip)) {
+controller_fail:
+			printk(KERN_ERR "HFC_multi ERROR, getting no 125us "
+			    "pulse. Seems that controller fails.\n");
+			err = -EIO;
+			goto out;
+		}
+		if (test_bit(HFC_CHIP_PCM_SLAVE, &hc->chip)) {
+			printk(KERN_INFO "controller is PCM bus SLAVE "
+				"(ignoring missing PCM clock)\n");
+		} else {
+			/* only one pcm master */
+			if (test_bit(HFC_CHIP_PLXSD, &hc->chip)
+				&& plxsd_master) {
+				printk(KERN_ERR "HFC_multi ERROR, no clock "
+				    "on another Speech Design card found. "
+				    "Please be sure to connect PCM cable.\n");
+				err = -EIO;
+				goto out;
+			}
+			/* retry with master clock */
+			if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+				spin_lock_irqsave(&plx_lock, plx_flags);
+				plx_acc_32 = (u_int *)(hc->plx_membase +
+					PLX_GPIOC);
+				pv = readl(plx_acc_32);
+				pv |= PLX_MASTER_EN | PLX_SLAVE_EN_N;
+				pv |= PLX_SYNC_O_EN;
+				writel(pv, plx_acc_32);
+				spin_unlock_irqrestore(&plx_lock, plx_flags);
+				if (debug & DEBUG_HFCMULTI_INIT)
+				    printk(KERN_WARNING "%s: master: PLX_GPIO"
+					"=%x\n", __func__, pv);
+			}
+			hc->hw.r_pcm_md0 |= V_PCM_MD;
+			HFC_outb(hc, R_PCM_MD0, hc->hw.r_pcm_md0 | 0x00);
+			spin_unlock_irqrestore(&hc->lock, flags);
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			schedule_timeout((HZ/100)?:1); /* Timeout min. 10ms */
+			spin_lock_irqsave(&hc->lock, flags);
+			val2 = HFC_inb(hc, R_F0_CNTL);
+			val2 += HFC_inb(hc, R_F0_CNTH) << 8;
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG "HFC_multi F0_CNT %ld after "
+					"10 ms (2nd try)\n", val2);
+			if (val2 >= val+8) { /* 1 ms */
+				test_and_set_bit(HFC_CHIP_PCM_MASTER,
+					&hc->chip);
+				printk(KERN_INFO "controller is PCM bus MASTER "
+					"(auto detected)\n");
+			} else
+				goto controller_fail;
+		}
+	}
+
+	/* Release the DSP Reset */
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip))
+			plxsd_master = 1;
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc_32 = (u_int *)(hc->plx_membase+PLX_GPIOC);
+		pv = readl(plx_acc_32);
+		pv |=  PLX_DSP_RES_N;
+		writel(pv, plx_acc_32);
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_WARNING "%s: reset off: PLX_GPIO=%x\n",
+				__func__, pv);
+	}
+
+	/* pcm id */
+	if (hc->pcm)
+		printk(KERN_INFO "controller has given PCM BUS ID %d\n",
+			hc->pcm);
+	else {
+		if (test_bit(HFC_CHIP_PCM_MASTER, &hc->chip)
+		 || test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			PCM_cnt++; /* SD has proprietary bridging */
+		}
+		hc->pcm = PCM_cnt;
+		printk(KERN_INFO "controller has PCM BUS ID %d "
+			"(auto selected)\n", hc->pcm);
+	}
+
+	/* set up timer */
+	HFC_outb(hc, R_TI_WD, poll_timer);
+	hc->hw.r_irqmsk_misc |= V_TI_IRQMSK;
+
+	/*
+	 * set up 125us interrupt, only if function pointer is available
+	 * and module parameter timer is set
+	 */
+	if (timer && hfc_interrupt && register_interrupt) {
+		/* only one chip should use this interrupt */
+		timer = 0;
+		interrupt_registered = 1;
+		hc->hw.r_irqmsk_misc |= V_PROC_IRQMSK;
+		/* deactivate other interrupts in ztdummy */
+		register_interrupt();
+	}
+
+	/* set E1 state machine IRQ */
+	if (hc->type == 1)
+		hc->hw.r_irqmsk_misc |= V_STA_IRQMSK;
+
+	/* set DTMF detection */
+	if (test_bit(HFC_CHIP_DTMF, &hc->chip)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: enabling DTMF detection "
+			    "for all B-channel\n", __func__);
+		hc->hw.r_dtmf = V_DTMF_EN | V_DTMF_STOP;
+		if (test_bit(HFC_CHIP_ULAW, &hc->chip))
+			hc->hw.r_dtmf |= V_ULAW_SEL;
+		HFC_outb(hc, R_DTMF_N, 102 - 1);
+		hc->hw.r_irqmsk_misc |= V_DTMF_IRQMSK;
+	}
+
+	/* conference engine */
+	if (test_bit(HFC_CHIP_ULAW, &hc->chip))
+		r_conf_en = V_CONF_EN | V_ULAW;
+	else
+		r_conf_en = V_CONF_EN;
+	HFC_outb(hc, R_CONF_EN, r_conf_en);
+
+	/* setting leds */
+	switch (hc->leds) {
+	case 1: /* HFC-E1 OEM */
+		if (test_bit(HFC_CHIP_WATCHDOG, &hc->chip))
+			HFC_outb(hc, R_GPIO_SEL, 0x32);
+		else
+			HFC_outb(hc, R_GPIO_SEL, 0x30);
+
+		HFC_outb(hc, R_GPIO_EN1, 0x0f);
+		HFC_outb(hc, R_GPIO_OUT1, 0x00);
+
+		HFC_outb(hc, R_GPIO_EN0, V_GPIO_EN2 | V_GPIO_EN3);
+		break;
+
+	case 2: /* HFC-4S OEM */
+	case 3:
+		HFC_outb(hc, R_GPIO_SEL, 0xf0);
+		HFC_outb(hc, R_GPIO_EN1, 0xff);
+		HFC_outb(hc, R_GPIO_OUT1, 0x00);
+		break;
+	}
+
+	/* set master clock */
+	if (hc->masterclk >= 0) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: setting ST master clock "
+			    "to port %d (0..%d)\n",
+			    __func__, hc->masterclk, hc->ports-1);
+		hc->hw.r_st_sync = hc->masterclk | V_AUTO_SYNC;
+		HFC_outb(hc, R_ST_SYNC, hc->hw.r_st_sync);
+	}
+
+	/* setting misc irq */
+	HFC_outb(hc, R_IRQMSK_MISC, hc->hw.r_irqmsk_misc);
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "r_irqmsk_misc.2: 0x%x\n",
+		    hc->hw.r_irqmsk_misc);
+
+	/* RAM access test */
+	HFC_outb(hc, R_RAM_ADDR0, 0);
+	HFC_outb(hc, R_RAM_ADDR1, 0);
+	HFC_outb(hc, R_RAM_ADDR2, 0);
+	for (i = 0; i < 256; i++) {
+		HFC_outb_nodebug(hc, R_RAM_ADDR0, i);
+		HFC_outb_nodebug(hc, R_RAM_DATA, ((i*3)&0xff));
+	}
+	for (i = 0; i < 256; i++) {
+		HFC_outb_nodebug(hc, R_RAM_ADDR0, i);
+		HFC_inb_nodebug(hc, R_RAM_DATA);
+		rval = HFC_inb_nodebug(hc, R_INT_DATA);
+		if (rval != ((i * 3) & 0xff)) {
+			printk(KERN_DEBUG
+			    "addr:%x val:%x should:%x\n", i, rval,
+			    (i * 3) & 0xff);
+			err++;
+		}
+	}
+	if (err) {
+		printk(KERN_DEBUG "aborting - %d RAM access errors\n", err);
+		err = -EIO;
+		goto out;
+	}
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: done\n", __func__);
+out:
+	spin_unlock_irqrestore(&hc->lock, flags);
+	return err;
+}
+
+
+/*
+ * control the watchdog
+ */
+static void
+hfcmulti_watchdog(struct hfc_multi *hc)
+{
+	hc->wdcount++;
+
+	if (hc->wdcount > 10) {
+		hc->wdcount = 0;
+		hc->wdbyte = hc->wdbyte == V_GPIO_OUT2 ?
+		    V_GPIO_OUT3 : V_GPIO_OUT2;
+
+	/* printk("Sending Watchdog Kill %x\n",hc->wdbyte); */
+		HFC_outb(hc, R_GPIO_EN0, V_GPIO_EN2 | V_GPIO_EN3);
+		HFC_outb(hc, R_GPIO_OUT0, hc->wdbyte);
+	}
+}
+
+
+
+/*
+ * output leds
+ */
+static void
+hfcmulti_leds(struct hfc_multi *hc)
+{
+	unsigned long lled;
+	unsigned long leddw;
+	int i, state, active, leds;
+	struct dchannel *dch;
+	int led[4];
+
+	hc->ledcount += poll;
+	if (hc->ledcount > 4096) {
+		hc->ledcount -= 4096;
+		hc->ledstate = 0xAFFEAFFE;
+	}
+
+	switch (hc->leds) {
+	case 1: /* HFC-E1 OEM */
+		/* 2 red blinking: NT mode deactivate
+		 * 2 red steady:   TE mode deactivate
+		 * left green:     L1 active
+		 * left red:       frame sync, but no L1
+		 * right green:    L2 active
+		 */
+		if (hc->chan[hc->dslot].sync != 2) { /* no frame sync */
+			if (hc->chan[hc->dslot].dch->dev.D.protocol
+				!= ISDN_P_NT_E1) {
+				led[0] = 1;
+				led[1] = 1;
+			} else if (hc->ledcount>>11) {
+				led[0] = 1;
+				led[1] = 1;
+			} else {
+				led[0] = 0;
+				led[1] = 0;
+			}
+			led[2] = 0;
+			led[3] = 0;
+		} else { /* with frame sync */
+			/* TODO make it work */
+			led[0] = 0;
+			led[1] = 0;
+			led[2] = 0;
+			led[3] = 1;
+		}
+		leds = (led[0] | (led[1]<<2) | (led[2]<<1) | (led[3]<<3))^0xF;
+			/* leds are inverted */
+		if (leds != (int)hc->ledstate) {
+			HFC_outb_nodebug(hc, R_GPIO_OUT1, leds);
+			hc->ledstate = leds;
+		}
+		break;
+
+	case 2: /* HFC-4S OEM */
+		/* red blinking = PH_DEACTIVATE NT Mode
+		 * red steady   = PH_DEACTIVATE TE Mode
+		 * green steady = PH_ACTIVATE
+		 */
+		for (i = 0; i < 4; i++) {
+			state = 0;
+			active = -1;
+			dch = hc->chan[(i << 2) | 2].dch;
+			if (dch) {
+				state = dch->state;
+				if (dch->dev.D.protocol == ISDN_P_NT_S0)
+					active = 3;
+				else
+					active = 7;
+			}
+			if (state) {
+				if (state == active) {
+					led[i] = 1; /* led green */
+				} else
+					if (dch->dev.D.protocol == ISDN_P_TE_S0)
+						/* TE mode: led red */
+						led[i] = 2;
+					else
+						if (hc->ledcount>>11)
+							/* led red */
+							led[i] = 2;
+						else
+							/* led off */
+							led[i] = 0;
+			} else
+				led[i] = 0; /* led off */
+		}
+		if (test_bit(HFC_CHIP_B410P, &hc->chip)) {
+			leds = 0;
+			for (i = 0; i < 4; i++) {
+				if (led[i] == 1) {
+					/*green*/
+					leds |= (0x2 << (i * 2));
+				} else if (led[i] == 2) {
+					/*red*/
+					leds |= (0x1 << (i * 2));
+				}
+			}
+			if (leds != (int)hc->ledstate) {
+				vpm_out(hc, 0, 0x1a8 + 3, leds);
+				hc->ledstate = leds;
+			}
+		} else {
+			leds = ((led[3] > 0) << 0) | ((led[1] > 0) << 1) |
+			    ((led[0] > 0) << 2) | ((led[2] > 0) << 3) |
+			    ((led[3] & 1) << 4) | ((led[1] & 1) << 5) |
+			    ((led[0] & 1) << 6) | ((led[2] & 1) << 7);
+			if (leds != (int)hc->ledstate) {
+				HFC_outb_nodebug(hc, R_GPIO_EN1, leds & 0x0F);
+				HFC_outb_nodebug(hc, R_GPIO_OUT1, leds >> 4);
+				hc->ledstate = leds;
+			}
+		}
+		break;
+
+	case 3: /* HFC 1S/2S Beronet */
+		/* red blinking = PH_DEACTIVATE NT Mode
+		 * red steady   = PH_DEACTIVATE TE Mode
+		 * green steady = PH_ACTIVATE
+		 */
+		for (i = 0; i < 2; i++) {
+			state = 0;
+			active = -1;
+			dch = hc->chan[(i << 2) | 2].dch;
+			if (dch) {
+				state = dch->state;
+				if (dch->dev.D.protocol == ISDN_P_NT_S0)
+					active = 3;
+				else
+					active = 7;
+			}
+			if (state) {
+				if (state == active) {
+					led[i] = 1; /* led green */
+				} else
+					if (dch->dev.D.protocol == ISDN_P_TE_S0)
+						/* TE mode: led red */
+						led[i] = 2;
+					else
+						if (hc->ledcount >> 11)
+							/* led red */
+							led[i] = 2;
+						else
+							/* led off */
+							led[i] = 0;
+			} else
+				led[i] = 0; /* led off */
+		}
+
+
+		leds = (led[0] > 0) | ((led[1] > 0)<<1) | ((led[0]&1)<<2)
+			| ((led[1]&1)<<3);
+		if (leds != (int)hc->ledstate) {
+			HFC_outb_nodebug(hc, R_GPIO_EN1,
+			    ((led[0] > 0) << 2) | ((led[1] > 0) << 3));
+			HFC_outb_nodebug(hc, R_GPIO_OUT1,
+			    ((led[0] & 1) << 2) | ((led[1] & 1) << 3));
+			hc->ledstate = leds;
+		}
+		break;
+	case 8: /* HFC 8S+ Beronet */
+		lled = 0;
+
+		for (i = 0; i < 8; i++) {
+			state = 0;
+			active = -1;
+			dch = hc->chan[(i << 2) | 2].dch;
+			if (dch) {
+				state = dch->state;
+				if (dch->dev.D.protocol == ISDN_P_NT_S0)
+					active = 3;
+				else
+					active = 7;
+			}
+			if (state) {
+				if (state == active) {
+					lled |= 0 << i;
+				} else
+					if (hc->ledcount >> 11)
+						lled |= 0 << i;
+					else
+						lled |= 1 << i;
+			} else
+				lled |= 1 << i;
+		}
+		leddw = lled << 24 | lled << 16 | lled << 8 | lled;
+		if (leddw != hc->ledstate) {
+			/* HFC_outb(hc, R_BRG_PCM_CFG, 1);
+			HFC_outb(c, R_BRG_PCM_CFG, (0x0 << 6) | 0x3); */
+			/* was _io before */
+			HFC_outb_nodebug(hc, R_BRG_PCM_CFG, 1 | V_PCM_CLK);
+			outw(0x4000, hc->pci_iobase + 4);
+			outl(leddw, hc->pci_iobase);
+			HFC_outb_nodebug(hc, R_BRG_PCM_CFG, V_PCM_CLK);
+			hc->ledstate = leddw;
+		}
+		break;
+	}
+}
+/*
+ * read dtmf coefficients
+ */
+
+static void
+hfcmulti_dtmf(struct hfc_multi *hc)
+{
+	s32		*coeff;
+	u_int		mantissa;
+	int		co, ch;
+	struct bchannel	*bch = NULL;
+	u8		exponent;
+	int		dtmf = 0;
+	int		addr;
+	u16		w_float;
+	struct sk_buff	*skb;
+	struct mISDNhead *hh;
+
+	if (debug & DEBUG_HFCMULTI_DTMF)
+		printk(KERN_DEBUG "%s: dtmf detection irq\n", __func__);
+	for (ch = 0; ch <= 31; ch++) {
+		/* only process enabled B-channels */
+		bch = hc->chan[ch].bch;
+		if (!bch)
+			continue;
+		if (!hc->created[hc->chan[ch].port])
+			continue;
+		if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
+			continue;
+		if (debug & DEBUG_HFCMULTI_DTMF)
+			printk(KERN_DEBUG "%s: dtmf channel %d:",
+				__func__, ch);
+		coeff = &(hc->chan[ch].coeff[hc->chan[ch].coeff_count * 16]);
+		dtmf = 1;
+		for (co = 0; co < 8; co++) {
+			/* read W(n-1) coefficient */
+			addr = hc->DTMFbase + ((co<<7) | (ch<<2));
+			HFC_outb_nodebug(hc, R_RAM_ADDR0, addr);
+			HFC_outb_nodebug(hc, R_RAM_ADDR1, addr>>8);
+			HFC_outb_nodebug(hc, R_RAM_ADDR2, (addr>>16)
+				| V_ADDR_INC);
+			w_float = HFC_inb_nodebug(hc, R_RAM_DATA);
+			w_float |= (HFC_inb_nodebug(hc, R_RAM_DATA) << 8);
+			if (debug & DEBUG_HFCMULTI_DTMF)
+				printk(" %04x", w_float);
+
+			/* decode float (see chip doc) */
+			mantissa = w_float & 0x0fff;
+			if (w_float & 0x8000)
+				mantissa |= 0xfffff000;
+			exponent = (w_float>>12) & 0x7;
+			if (exponent) {
+				mantissa ^= 0x1000;
+				mantissa <<= (exponent-1);
+			}
+
+			/* store coefficient */
+			coeff[co<<1] = mantissa;
+
+			/* read W(n) coefficient */
+			w_float = HFC_inb_nodebug(hc, R_RAM_DATA);
+			w_float |= (HFC_inb_nodebug(hc, R_RAM_DATA) << 8);
+			if (debug & DEBUG_HFCMULTI_DTMF)
+				printk(" %04x", w_float);
+
+			/* decode float (see chip doc) */
+			mantissa = w_float & 0x0fff;
+			if (w_float & 0x8000)
+				mantissa |= 0xfffff000;
+			exponent = (w_float>>12) & 0x7;
+			if (exponent) {
+				mantissa ^= 0x1000;
+				mantissa <<= (exponent-1);
+			}
+
+			/* store coefficient */
+			coeff[(co<<1)|1] = mantissa;
+		}
+		if (debug & DEBUG_HFCMULTI_DTMF)
+			printk("%s: DTMF ready %08x %08x %08x %08x "
+			    "%08x %08x %08x %08x\n", __func__,
+			    coeff[0], coeff[1], coeff[2], coeff[3],
+			    coeff[4], coeff[5], coeff[6], coeff[7]);
+		hc->chan[ch].coeff_count++;
+		if (hc->chan[ch].coeff_count == 8) {
+			hc->chan[ch].coeff_count = 0;
+			skb = mI_alloc_skb(512, GFP_ATOMIC);
+			if (!skb) {
+				printk(KERN_WARNING "%s: No memory for skb\n",
+				    __func__);
+				continue;
+			}
+			hh = mISDN_HEAD_P(skb);
+			hh->prim = PH_CONTROL_IND;
+			hh->id = DTMF_HFC_COEF;
+			memcpy(skb_put(skb, 512), hc->chan[ch].coeff, 512);
+			recv_Bchannel_skb(bch, skb);
+		}
+	}
+
+	/* restart DTMF processing */
+	hc->dtmf = dtmf;
+	if (dtmf)
+		HFC_outb_nodebug(hc, R_DTMF, hc->hw.r_dtmf | V_RST_DTMF);
+}
+
+
+/*
+ * fill fifo as much as possible
+ */
+
+static void
+hfcmulti_tx(struct hfc_multi *hc, int ch)
+{
+	int i, ii, temp, len = 0;
+	int Zspace, z1, z2; /* must be int for calculation */
+	int Fspace, f1, f2;
+	u_char *d;
+	int *txpending, slot_tx;
+	struct	bchannel *bch;
+	struct  dchannel *dch;
+	struct  sk_buff **sp = NULL;
+	int *idxp;
+
+	bch = hc->chan[ch].bch;
+	dch = hc->chan[ch].dch;
+	if ((!dch) && (!bch))
+		return;
+
+	txpending = &hc->chan[ch].txpending;
+	slot_tx = hc->chan[ch].slot_tx;
+	if (dch) {
+		if (!test_bit(FLG_ACTIVE, &dch->Flags))
+			return;
+		sp = &dch->tx_skb;
+		idxp = &dch->tx_idx;
+	} else {
+		if (!test_bit(FLG_ACTIVE, &bch->Flags))
+			return;
+		sp = &bch->tx_skb;
+		idxp = &bch->tx_idx;
+	}
+	if (*sp)
+		len = (*sp)->len;
+
+	if ((!len) && *txpending != 1)
+		return; /* no data */
+
+	if (test_bit(HFC_CHIP_B410P, &hc->chip) &&
+	    (hc->chan[ch].protocol == ISDN_P_B_RAW) &&
+	    (hc->chan[ch].slot_rx < 0) &&
+	    (hc->chan[ch].slot_tx < 0))
+		HFC_outb_nodebug(hc, R_FIFO, 0x20 | (ch << 1));
+	else
+		HFC_outb_nodebug(hc, R_FIFO, ch << 1);
+	HFC_wait_nodebug(hc);
+
+	if (*txpending == 2) {
+		/* reset fifo */
+		HFC_outb_nodebug(hc, R_INC_RES_FIFO, V_RES_F);
+		HFC_wait_nodebug(hc);
+		HFC_outb(hc, A_SUBCH_CFG, 0);
+		*txpending = 1;
+	}
+next_frame:
+	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
+		f1 = HFC_inb_nodebug(hc, A_F1);
+		f2 = HFC_inb_nodebug(hc, A_F2);
+		while (f2 != (temp = HFC_inb_nodebug(hc, A_F2))) {
+			if (debug & DEBUG_HFCMULTI_FIFO)
+				printk(KERN_DEBUG
+				    "%s(card %d): reread f2 because %d!=%d\n",
+				    __func__, hc->id + 1, temp, f2);
+			f2 = temp; /* repeat until F2 is equal */
+		}
+		Fspace = f2 - f1 - 1;
+		if (Fspace < 0)
+			Fspace += hc->Flen;
+		/*
+		 * Old FIFO handling doesn't give us the current Z2 read
+		 * pointer, so we cannot send the next frame before the fifo
+		 * is empty. It makes no difference except for a slightly
+		 * lower performance.
+		 */
+		if (test_bit(HFC_CHIP_REVISION0, &hc->chip)) {
+			if (f1 != f2)
+				Fspace = 0;
+			else
+				Fspace = 1;
+		}
+		/* one frame only for ST D-channels, to allow resending */
+		if (hc->type != 1 && dch) {
+			if (f1 != f2)
+				Fspace = 0;
+		}
+		/* F-counter full condition */
+		if (Fspace == 0)
+			return;
+	}
+	z1 = HFC_inw_nodebug(hc, A_Z1) - hc->Zmin;
+	z2 = HFC_inw_nodebug(hc, A_Z2) - hc->Zmin;
+	while (z2 != (temp = (HFC_inw_nodebug(hc, A_Z2) - hc->Zmin))) {
+		if (debug & DEBUG_HFCMULTI_FIFO)
+			printk(KERN_DEBUG "%s(card %d): reread z2 because "
+				"%d!=%d\n", __func__, hc->id + 1, temp, z2);
+		z2 = temp; /* repeat unti Z2 is equal */
+	}
+	Zspace = z2 - z1;
+	if (Zspace <= 0)
+		Zspace += hc->Zlen;
+	Zspace -= 4; /* keep not too full, so pointers will not overrun */
+	/* fill transparent data only to maxinum transparent load (minus 4) */
+	if (bch && test_bit(FLG_TRANSPARENT, &bch->Flags))
+		Zspace = Zspace - hc->Zlen + hc->max_trans;
+	if (Zspace <= 0) /* no space of 4 bytes */
+		return;
+
+	/* if no data */
+	if (!len) {
+		if (z1 == z2) { /* empty */
+			/* if done with FIFO audio data during PCM connection */
+			if (bch && (!test_bit(FLG_HDLC, &bch->Flags)) &&
+			    *txpending && slot_tx >= 0) {
+				if (debug & DEBUG_HFCMULTI_MODE)
+					printk(KERN_DEBUG
+					    "%s: reconnecting PCM due to no "
+					    "more FIFO data: channel %d "
+					    "slot_tx %d\n",
+					    __func__, ch, slot_tx);
+				/* connect slot */
+				HFC_outb(hc, A_CON_HDLC, 0xc0 | 0x00 |
+				    V_HDLC_TRP | V_IFF);
+				HFC_outb_nodebug(hc, R_FIFO, ch<<1 | 1);
+				HFC_wait_nodebug(hc);
+				HFC_outb(hc, A_CON_HDLC, 0xc0 | 0x00 |
+				    V_HDLC_TRP | V_IFF);
+				HFC_outb_nodebug(hc, R_FIFO, ch<<1);
+				HFC_wait_nodebug(hc);
+			}
+			*txpending = 0;
+		}
+		return; /* no data */
+	}
+
+	/* if audio data and connected slot */
+	if (bch && (!test_bit(FLG_HDLC, &bch->Flags)) && (!*txpending)
+		&& slot_tx >= 0) {
+		if (debug & DEBUG_HFCMULTI_MODE)
+			printk(KERN_DEBUG "%s: disconnecting PCM due to "
+			    "FIFO data: channel %d slot_tx %d\n",
+			    __func__, ch, slot_tx);
+		/* disconnect slot */
+		HFC_outb(hc, A_CON_HDLC, 0x80 | 0x00 | V_HDLC_TRP | V_IFF);
+		HFC_outb_nodebug(hc, R_FIFO, ch<<1 | 1);
+		HFC_wait_nodebug(hc);
+		HFC_outb(hc, A_CON_HDLC, 0x80 | 0x00 | V_HDLC_TRP | V_IFF);
+		HFC_outb_nodebug(hc, R_FIFO, ch<<1);
+		HFC_wait_nodebug(hc);
+	}
+	*txpending = 1;
+
+	/* show activity */
+	hc->activity[hc->chan[ch].port] = 1;
+
+	/* fill fifo to what we have left */
+	ii = len;
+	if (dch || test_bit(FLG_HDLC, &bch->Flags))
+		temp = 1;
+	else
+		temp = 0;
+	i = *idxp;
+	d = (*sp)->data + i;
+	if (ii - i > Zspace)
+		ii = Zspace + i;
+	if (debug & DEBUG_HFCMULTI_FIFO)
+		printk(KERN_DEBUG "%s(card %d): fifo(%d) has %d bytes space "
+		    "left (z1=%04x, z2=%04x) sending %d of %d bytes %s\n",
+			__func__, hc->id + 1, ch, Zspace, z1, z2, ii-i, len-i,
+			temp ? "HDLC":"TRANS");
+
+
+	/* Have to prep the audio data */
+	hc->write_fifo(hc, d, ii - i);
+	*idxp = ii;
+
+	/* if not all data has been written */
+	if (ii != len) {
+		/* NOTE: fifo is started by the calling function */
+		return;
+	}
+
+	/* if all data has been written, terminate frame */
+	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
+		/* increment f-counter */
+		HFC_outb_nodebug(hc, R_INC_RES_FIFO, V_INC_F);
+		HFC_wait_nodebug(hc);
+	}
+
+	/* send confirm, since get_net_bframe will not do it with trans */
+	if (bch && test_bit(FLG_TRANSPARENT, &bch->Flags))
+		confirm_Bsend(bch);
+
+	/* check for next frame */
+	dev_kfree_skb(*sp);
+	if (bch && get_next_bframe(bch)) { /* hdlc is confirmed here */
+		len = (*sp)->len;
+		goto next_frame;
+	}
+	if (dch && get_next_dframe(dch)) {
+		len = (*sp)->len;
+		goto next_frame;
+	}
+
+	/*
+	 * now we have no more data, so in case of transparent,
+	 * we set the last byte in fifo to 'silence' in case we will get
+	 * no more data at all. this prevents sending an undefined value.
+	 */
+	if (bch && test_bit(FLG_TRANSPARENT, &bch->Flags))
+		HFC_outb_nodebug(hc, A_FIFO_DATA0_NOINC, silence);
+}
+
+
+/* NOTE: only called if E1 card is in active state */
+static void
+hfcmulti_rx(struct hfc_multi *hc, int ch)
+{
+	int temp;
+	int Zsize, z1, z2 = 0; /* = 0, to make GCC happy */
+	int f1 = 0, f2 = 0; /* = 0, to make GCC happy */
+	int again = 0;
+	struct	bchannel *bch;
+	struct  dchannel *dch;
+	struct sk_buff	*skb, **sp = NULL;
+	int	maxlen;
+
+	bch = hc->chan[ch].bch;
+	dch = hc->chan[ch].dch;
+	if ((!dch) && (!bch))
+		return;
+	if (dch) {
+		if (!test_bit(FLG_ACTIVE, &dch->Flags))
+			return;
+		sp = &dch->rx_skb;
+		maxlen = dch->maxlen;
+	} else {
+		if (!test_bit(FLG_ACTIVE, &bch->Flags))
+			return;
+		sp = &bch->rx_skb;
+		maxlen = bch->maxlen;
+	}
+next_frame:
+	/* on first AND before getting next valid frame, R_FIFO must be written
+	   to. */
+	if (test_bit(HFC_CHIP_B410P, &hc->chip) &&
+	    (hc->chan[ch].protocol == ISDN_P_B_RAW) &&
+	    (hc->chan[ch].slot_rx < 0) &&
+	    (hc->chan[ch].slot_tx < 0))
+		HFC_outb_nodebug(hc, R_FIFO, 0x20 | (ch<<1) | 1);
+	else
+		HFC_outb_nodebug(hc, R_FIFO, (ch<<1)|1);
+	HFC_wait_nodebug(hc);
+
+	/* ignore if rx is off BUT change fifo (above) to start pending TX */
+	if (hc->chan[ch].rx_off)
+		return;
+
+	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
+		f1 = HFC_inb_nodebug(hc, A_F1);
+		while (f1 != (temp = HFC_inb_nodebug(hc, A_F1))) {
+			if (debug & DEBUG_HFCMULTI_FIFO)
+				printk(KERN_DEBUG
+				    "%s(card %d): reread f1 because %d!=%d\n",
+				    __func__, hc->id + 1, temp, f1);
+			f1 = temp; /* repeat until F1 is equal */
+		}
+		f2 = HFC_inb_nodebug(hc, A_F2);
+	}
+	z1 = HFC_inw_nodebug(hc, A_Z1) - hc->Zmin;
+	while (z1 != (temp = (HFC_inw_nodebug(hc, A_Z1) - hc->Zmin))) {
+		if (debug & DEBUG_HFCMULTI_FIFO)
+			printk(KERN_DEBUG "%s(card %d): reread z2 because "
+				"%d!=%d\n", __func__, hc->id + 1, temp, z2);
+		z1 = temp; /* repeat until Z1 is equal */
+	}
+	z2 = HFC_inw_nodebug(hc, A_Z2) - hc->Zmin;
+	Zsize = z1 - z2;
+	if ((dch || test_bit(FLG_HDLC, &bch->Flags)) && f1 != f2)
+		/* complete hdlc frame */
+		Zsize++;
+	if (Zsize < 0)
+		Zsize += hc->Zlen;
+	/* if buffer is empty */
+	if (Zsize <= 0)
+		return;
+
+	if (*sp == NULL) {
+		*sp = mI_alloc_skb(maxlen + 3, GFP_ATOMIC);
+		if (*sp == NULL) {
+			printk(KERN_DEBUG "%s: No mem for rx_skb\n",
+			    __func__);
+			return;
+		}
+	}
+	/* show activity */
+	hc->activity[hc->chan[ch].port] = 1;
+
+	/* empty fifo with what we have */
+	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
+		if (debug & DEBUG_HFCMULTI_FIFO)
+			printk(KERN_DEBUG "%s(card %d): fifo(%d) reading %d "
+			    "bytes (z1=%04x, z2=%04x) HDLC %s (f1=%d, f2=%d) "
+			    "got=%d (again %d)\n", __func__, hc->id + 1, ch,
+			    Zsize, z1, z2, (f1 == f2) ? "fragment" : "COMPLETE",
+			    f1, f2, Zsize + (*sp)->len, again);
+		/* HDLC */
+		if ((Zsize + (*sp)->len) > (maxlen + 3)) {
+			if (debug & DEBUG_HFCMULTI_FIFO)
+				printk(KERN_DEBUG
+				    "%s(card %d): hdlc-frame too large.\n",
+				    __func__, hc->id + 1);
+			skb_trim(*sp, 0);
+			HFC_outb_nodebug(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait_nodebug(hc);
+			return;
+		}
+
+		hc->read_fifo(hc, skb_put(*sp, Zsize), Zsize);
+
+		if (f1 != f2) {
+			/* increment Z2,F2-counter */
+			HFC_outb_nodebug(hc, R_INC_RES_FIFO, V_INC_F);
+			HFC_wait_nodebug(hc);
+			/* check size */
+			if ((*sp)->len < 4) {
+				if (debug & DEBUG_HFCMULTI_FIFO)
+					printk(KERN_DEBUG
+					    "%s(card %d): Frame below minimum "
+					    "size\n", __func__, hc->id + 1);
+				skb_trim(*sp, 0);
+				goto next_frame;
+			}
+			/* there is at least one complete frame, check crc */
+			if ((*sp)->data[(*sp)->len - 1]) {
+				if (debug & DEBUG_HFCMULTI_CRC)
+					printk(KERN_DEBUG
+					    "%s: CRC-error\n", __func__);
+				skb_trim(*sp, 0);
+				goto next_frame;
+			}
+			skb_trim(*sp, (*sp)->len - 3);
+			if ((*sp)->len < MISDN_COPY_SIZE) {
+				skb = *sp;
+				*sp = mI_alloc_skb(skb->len, GFP_ATOMIC);
+				if (*sp) {
+					memcpy(skb_put(*sp, skb->len),
+					    skb->data, skb->len);
+					skb_trim(skb, 0);
+				} else {
+					printk(KERN_DEBUG "%s: No mem\n",
+					    __func__);
+					*sp = skb;
+					skb = NULL;
+				}
+			} else {
+				skb = NULL;
+			}
+			if (debug & DEBUG_HFCMULTI_FIFO) {
+				printk(KERN_DEBUG "%s(card %d):",
+					__func__, hc->id + 1);
+				temp = 0;
+				while (temp < (*sp)->len)
+					printk(" %02x", (*sp)->data[temp++]);
+				printk("\n");
+			}
+			if (dch)
+				recv_Dchannel(dch);
+			else
+				recv_Bchannel(bch);
+			*sp = skb;
+			again++;
+			goto next_frame;
+		}
+		/* there is an incomplete frame */
+	} else {
+		/* transparent */
+		if (Zsize > skb_tailroom(*sp))
+			Zsize = skb_tailroom(*sp);
+		hc->read_fifo(hc, skb_put(*sp, Zsize), Zsize);
+		if (((*sp)->len) < MISDN_COPY_SIZE) {
+			skb = *sp;
+			*sp = mI_alloc_skb(skb->len, GFP_ATOMIC);
+			if (*sp) {
+				memcpy(skb_put(*sp, skb->len),
+				    skb->data, skb->len);
+				skb_trim(skb, 0);
+			} else {
+				printk(KERN_DEBUG "%s: No mem\n", __func__);
+				*sp = skb;
+				skb = NULL;
+			}
+		} else {
+			skb = NULL;
+		}
+		if (debug & DEBUG_HFCMULTI_FIFO)
+			printk(KERN_DEBUG
+			    "%s(card %d): fifo(%d) reading %d bytes "
+			    "(z1=%04x, z2=%04x) TRANS\n",
+				__func__, hc->id + 1, ch, Zsize, z1, z2);
+		/* only bch is transparent */
+		recv_Bchannel(bch);
+		*sp = skb;
+	}
+}
+
+
+/*
+ * Interrupt handler
+ */
+static void
+signal_state_up(struct dchannel *dch, int info, char *msg)
+{
+	struct sk_buff	*skb;
+	int		id, data = info;
+
+	if (debug & DEBUG_HFCMULTI_STATE)
+		printk(KERN_DEBUG "%s: %s\n", __func__, msg);
+
+	id = TEI_SAPI | (GROUP_TEI << 8); /* manager address */
+
+	skb = _alloc_mISDN_skb(MPH_INFORMATION_IND, id, sizeof(data), &data,
+		GFP_ATOMIC);
+	if (!skb)
+		return;
+	recv_Dchannel_skb(dch, skb);
+}
+
+static inline void
+handle_timer_irq(struct hfc_multi *hc)
+{
+	int		ch, temp;
+	struct dchannel	*dch;
+	u_long		flags;
+
+	/* process queued resync jobs */
+	if (hc->e1_resync) {
+		/* lock, so e1_resync gets not changed */
+		spin_lock_irqsave(&HFClock, flags);
+		if (hc->e1_resync & 1) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG "Enable SYNC_I\n");
+			HFC_outb(hc, R_SYNC_CTRL, V_EXT_CLK_SYNC);
+			/* disable JATT, if RX_SYNC is set */
+			if (test_bit(HFC_CHIP_RX_SYNC, &hc->chip))
+				HFC_outb(hc, R_SYNC_OUT, V_SYNC_E1_RX);
+		}
+		if (hc->e1_resync & 2) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG "Enable jatt PLL\n");
+			HFC_outb(hc, R_SYNC_CTRL, V_SYNC_OFFS);
+		}
+		if (hc->e1_resync & 4) {
+			if (debug & DEBUG_HFCMULTI_PLXSD)
+				printk(KERN_DEBUG
+				    "Enable QUARTZ for HFC-E1\n");
+			/* set jatt to quartz */
+			HFC_outb(hc, R_SYNC_CTRL, V_EXT_CLK_SYNC
+				| V_JATT_OFF);
+			/* switch to JATT, in case it is not already */
+			HFC_outb(hc, R_SYNC_OUT, 0);
+		}
+		hc->e1_resync = 0;
+		spin_unlock_irqrestore(&HFClock, flags);
+	}
+
+	if (hc->type != 1 || hc->e1_state == 1)
+		for (ch = 0; ch <= 31; ch++) {
+			if (hc->created[hc->chan[ch].port]) {
+				hfcmulti_tx(hc, ch);
+				/* fifo is started when switching to rx-fifo */
+				hfcmulti_rx(hc, ch);
+				if (hc->chan[ch].dch &&
+				    hc->chan[ch].nt_timer > -1) {
+					dch = hc->chan[ch].dch;
+					if (!(--hc->chan[ch].nt_timer)) {
+						schedule_event(dch,
+						    FLG_PHCHANGE);
+						if (debug &
+						    DEBUG_HFCMULTI_STATE)
+							printk(KERN_DEBUG
+							    "%s: nt_timer at "
+							    "state %x\n",
+							    __func__,
+							    dch->state);
+					}
+				}
+			}
+		}
+	if (hc->type == 1 && hc->created[0]) {
+		dch = hc->chan[hc->dslot].dch;
+		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dslot].cfg)) {
+			/* LOS */
+			temp = HFC_inb_nodebug(hc, R_SYNC_STA) & V_SIG_LOS;
+			if (!temp && hc->chan[hc->dslot].los)
+				signal_state_up(dch, L1_SIGNAL_LOS_ON,
+				    "LOS detected");
+			if (temp && !hc->chan[hc->dslot].los)
+				signal_state_up(dch, L1_SIGNAL_LOS_OFF,
+				    "LOS gone");
+			hc->chan[hc->dslot].los = temp;
+		}
+		if (test_bit(HFC_CFG_REPORT_AIS, &hc->chan[hc->dslot].cfg)) {
+			/* AIS */
+			temp = HFC_inb_nodebug(hc, R_SYNC_STA) & V_AIS;
+			if (!temp && hc->chan[hc->dslot].ais)
+				signal_state_up(dch, L1_SIGNAL_AIS_ON,
+				    "AIS detected");
+			if (temp && !hc->chan[hc->dslot].ais)
+				signal_state_up(dch, L1_SIGNAL_AIS_OFF,
+				    "AIS gone");
+			hc->chan[hc->dslot].ais = temp;
+		}
+		if (test_bit(HFC_CFG_REPORT_SLIP, &hc->chan[hc->dslot].cfg)) {
+			/* SLIP */
+			temp = HFC_inb_nodebug(hc, R_SLIP) & V_FOSLIP_RX;
+			if (!temp && hc->chan[hc->dslot].slip_rx)
+				signal_state_up(dch, L1_SIGNAL_SLIP_RX,
+				    " bit SLIP detected RX");
+			hc->chan[hc->dslot].slip_rx = temp;
+			temp = HFC_inb_nodebug(hc, R_SLIP) & V_FOSLIP_TX;
+			if (!temp && hc->chan[hc->dslot].slip_tx)
+				signal_state_up(dch, L1_SIGNAL_SLIP_TX,
+				    " bit SLIP detected TX");
+			hc->chan[hc->dslot].slip_tx = temp;
+		}
+		if (test_bit(HFC_CFG_REPORT_RDI, &hc->chan[hc->dslot].cfg)) {
+			/* RDI */
+			temp = HFC_inb_nodebug(hc, R_RX_SL0_0) & V_A;
+			if (!temp && hc->chan[hc->dslot].rdi)
+				signal_state_up(dch, L1_SIGNAL_RDI_ON,
+				    "RDI detected");
+			if (temp && !hc->chan[hc->dslot].rdi)
+				signal_state_up(dch, L1_SIGNAL_RDI_OFF,
+				    "RDI gone");
+			hc->chan[hc->dslot].rdi = temp;
+		}
+		temp = HFC_inb_nodebug(hc, R_JATT_DIR);
+		switch (hc->chan[hc->dslot].sync) {
+		case 0:
+			if ((temp & 0x60) == 0x60) {
+				if (debug & DEBUG_HFCMULTI_SYNC)
+					printk(KERN_DEBUG
+					    "%s: (id=%d) E1 now "
+					    "in clock sync\n",
+					    __func__, hc->id);
+				HFC_outb(hc, R_RX_OFF,
+				    hc->chan[hc->dslot].jitter | V_RX_INIT);
+				HFC_outb(hc, R_TX_OFF,
+				    hc->chan[hc->dslot].jitter | V_RX_INIT);
+				hc->chan[hc->dslot].sync = 1;
+				goto check_framesync;
+			}
+			break;
+		case 1:
+			if ((temp & 0x60) != 0x60) {
+				if (debug & DEBUG_HFCMULTI_SYNC)
+					printk(KERN_DEBUG
+					    "%s: (id=%d) E1 "
+					    "lost clock sync\n",
+					    __func__, hc->id);
+				hc->chan[hc->dslot].sync = 0;
+				break;
+			}
+check_framesync:
+			temp = HFC_inb_nodebug(hc, R_SYNC_STA);
+			if (temp == 0x27) {
+				if (debug & DEBUG_HFCMULTI_SYNC)
+					printk(KERN_DEBUG
+					    "%s: (id=%d) E1 "
+					    "now in frame sync\n",
+					    __func__, hc->id);
+				hc->chan[hc->dslot].sync = 2;
+			}
+			break;
+		case 2:
+			if ((temp & 0x60) != 0x60) {
+				if (debug & DEBUG_HFCMULTI_SYNC)
+					printk(KERN_DEBUG
+					    "%s: (id=%d) E1 lost "
+					    "clock & frame sync\n",
+					    __func__, hc->id);
+				hc->chan[hc->dslot].sync = 0;
+				break;
+			}
+			temp = HFC_inb_nodebug(hc, R_SYNC_STA);
+			if (temp != 0x27) {
+				if (debug & DEBUG_HFCMULTI_SYNC)
+					printk(KERN_DEBUG
+					    "%s: (id=%d) E1 "
+					    "lost frame sync\n",
+					    __func__, hc->id);
+				hc->chan[hc->dslot].sync = 1;
+			}
+			break;
+		}
+	}
+
+	if (test_bit(HFC_CHIP_WATCHDOG, &hc->chip))
+		hfcmulti_watchdog(hc);
+
+	if (hc->leds)
+		hfcmulti_leds(hc);
+}
+
+static void
+ph_state_irq(struct hfc_multi *hc, u_char r_irq_statech)
+{
+	struct dchannel	*dch;
+	int		ch;
+	int		active;
+	u_char		st_status, temp;
+
+	/* state machine */
+	for (ch = 0; ch <= 31; ch++) {
+		if (hc->chan[ch].dch) {
+			dch = hc->chan[ch].dch;
+			if (r_irq_statech & 1) {
+				HFC_outb_nodebug(hc, R_ST_SEL,
+					hc->chan[ch].port);
+				/* undocumented: delay after R_ST_SEL */
+				udelay(1);
+				/* undocumented: status changes during read */
+				st_status = HFC_inb_nodebug(hc, A_ST_RD_STATE);
+				while (st_status != (temp =
+					HFC_inb_nodebug(hc, A_ST_RD_STATE))) {
+					if (debug & DEBUG_HFCMULTI_STATE)
+						printk(KERN_DEBUG "%s: reread "
+						    "STATE because %d!=%d\n",
+						    __func__, temp,
+						    st_status);
+					st_status = temp; /* repeat */
+				}
+
+				/* Speech Design TE-sync indication */
+				if (test_bit(HFC_CHIP_PLXSD, &hc->chip) &&
+					dch->dev.D.protocol == ISDN_P_TE_S0) {
+					if (st_status & V_FR_SYNC_ST)
+						hc->syncronized |=
+						    (1 << hc->chan[ch].port);
+					else
+						hc->syncronized &=
+						   ~(1 << hc->chan[ch].port);
+				}
+				dch->state = st_status & 0x0f;
+				if (dch->dev.D.protocol == ISDN_P_NT_S0)
+					active = 3;
+				else
+					active = 7;
+				if (dch->state == active) {
+					HFC_outb_nodebug(hc, R_FIFO,
+						(ch << 1) | 1);
+					HFC_wait_nodebug(hc);
+					HFC_outb_nodebug(hc,
+						R_INC_RES_FIFO, V_RES_F);
+					HFC_wait_nodebug(hc);
+					dch->tx_idx = 0;
+				}
+				schedule_event(dch, FLG_PHCHANGE);
+				if (debug & DEBUG_HFCMULTI_STATE)
+					printk(KERN_DEBUG
+					    "%s: S/T newstate %x port %d\n",
+					    __func__, dch->state,
+					    hc->chan[ch].port);
+			}
+			r_irq_statech >>= 1;
+		}
+	}
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip))
+		plxsd_checksync(hc, 0);
+}
+
+static void
+fifo_irq(struct hfc_multi *hc, int block)
+{
+	int	ch, j;
+	struct dchannel	*dch;
+	struct bchannel	*bch;
+	u_char r_irq_fifo_bl;
+
+	r_irq_fifo_bl = HFC_inb_nodebug(hc, R_IRQ_FIFO_BL0 + block);
+	j = 0;
+	while (j < 8) {
+		ch = (block << 2) + (j >> 1);
+		dch = hc->chan[ch].dch;
+		bch = hc->chan[ch].bch;
+		if (((!dch) && (!bch)) || (!hc->created[hc->chan[ch].port])) {
+			j += 2;
+			continue;
+		}
+		if (dch && (r_irq_fifo_bl & (1 << j)) &&
+		    test_bit(FLG_ACTIVE, &dch->Flags)) {
+			hfcmulti_tx(hc, ch);
+			/* start fifo */
+			HFC_outb_nodebug(hc, R_FIFO, 0);
+			HFC_wait_nodebug(hc);
+		}
+		if (bch && (r_irq_fifo_bl & (1 << j)) &&
+		    test_bit(FLG_ACTIVE, &bch->Flags)) {
+			hfcmulti_tx(hc, ch);
+			/* start fifo */
+			HFC_outb_nodebug(hc, R_FIFO, 0);
+			HFC_wait_nodebug(hc);
+		}
+		j++;
+		if (dch && (r_irq_fifo_bl & (1 << j)) &&
+		    test_bit(FLG_ACTIVE, &dch->Flags)) {
+			hfcmulti_rx(hc, ch);
+		}
+		if (bch && (r_irq_fifo_bl & (1 << j)) &&
+		    test_bit(FLG_ACTIVE, &bch->Flags)) {
+			hfcmulti_rx(hc, ch);
+		}
+		j++;
+	}
+}
+
+#ifdef IRQ_DEBUG
+int irqsem;
+#endif
+static irqreturn_t
+hfcmulti_interrupt(int intno, void *dev_id)
+{
+#ifdef IRQCOUNT_DEBUG
+	static int iq1 = 0, iq2 = 0, iq3 = 0, iq4 = 0,
+	    iq5 = 0, iq6 = 0, iqcnt = 0;
+#endif
+	static int		count;
+	struct hfc_multi	*hc = dev_id;
+	struct dchannel		*dch;
+	u_char			r_irq_statech, status, r_irq_misc, r_irq_oview;
+	int			i;
+	u_short			*plx_acc, wval;
+	u_char			e1_syncsta, temp;
+	u_long			flags;
+
+	if (!hc) {
+		printk(KERN_ERR "HFC-multi: Spurious interrupt!\n");
+		return IRQ_NONE;
+	}
+
+	spin_lock(&hc->lock);
+
+#ifdef IRQ_DEBUG
+	if (irqsem)
+		printk(KERN_ERR "irq for card %d during irq from "
+		"card %d, this is no bug.\n", hc->id + 1, irqsem);
+	irqsem = hc->id + 1;
+#endif
+
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		spin_lock_irqsave(&plx_lock, flags);
+		plx_acc = (u_short *)(hc->plx_membase + PLX_INTCSR);
+		wval = readw(plx_acc);
+		spin_unlock_irqrestore(&plx_lock, flags);
+		if (!(wval & PLX_INTCSR_LINTI1_STATUS))
+			goto irq_notforus;
+	}
+
+	status = HFC_inb_nodebug(hc, R_STATUS);
+	r_irq_statech = HFC_inb_nodebug(hc, R_IRQ_STATECH);
+#ifdef IRQCOUNT_DEBUG
+	if (r_irq_statech)
+		iq1++;
+	if (status & V_DTMF_STA)
+		iq2++;
+	if (status & V_LOST_STA)
+		iq3++;
+	if (status & V_EXT_IRQSTA)
+		iq4++;
+	if (status & V_MISC_IRQSTA)
+		iq5++;
+	if (status & V_FR_IRQSTA)
+		iq6++;
+	if (iqcnt++ > 5000) {
+		printk(KERN_ERR "iq1:%x iq2:%x iq3:%x iq4:%x iq5:%x iq6:%x\n",
+		    iq1, iq2, iq3, iq4, iq5, iq6);
+		iqcnt = 0;
+	}
+#endif
+	if (!r_irq_statech &&
+	    !(status & (V_DTMF_STA | V_LOST_STA | V_EXT_IRQSTA |
+	    V_MISC_IRQSTA | V_FR_IRQSTA))) {
+		/* irq is not for us */
+		goto irq_notforus;
+	}
+	hc->irqcnt++;
+	if (r_irq_statech) {
+		if (hc->type != 1)
+			ph_state_irq(hc, r_irq_statech);
+	}
+	if (status & V_EXT_IRQSTA)
+		; /* external IRQ */
+	if (status & V_LOST_STA) {
+		/* LOST IRQ */
+		HFC_outb(hc, R_INC_RES_FIFO, V_RES_LOST); /* clear irq! */
+	}
+	if (status & V_MISC_IRQSTA) {
+		/* misc IRQ */
+		r_irq_misc = HFC_inb_nodebug(hc, R_IRQ_MISC);
+		if (r_irq_misc & V_STA_IRQ) {
+			if (hc->type == 1) {
+				/* state machine */
+				dch = hc->chan[hc->dslot].dch;
+				e1_syncsta = HFC_inb_nodebug(hc, R_SYNC_STA);
+				if (test_bit(HFC_CHIP_PLXSD, &hc->chip)
+				 && hc->e1_getclock) {
+					if (e1_syncsta & V_FR_SYNC_E1)
+						hc->syncronized = 1;
+					else
+						hc->syncronized = 0;
+				}
+				/* undocumented: status changes during read */
+				dch->state = HFC_inb_nodebug(hc, R_E1_RD_STA);
+				while (dch->state != (temp =
+					HFC_inb_nodebug(hc, R_E1_RD_STA))) {
+					if (debug & DEBUG_HFCMULTI_STATE)
+						printk(KERN_DEBUG "%s: reread "
+						    "STATE because %d!=%d\n",
+						    __func__, temp,
+						    dch->state);
+					dch->state = temp; /* repeat */
+				}
+				dch->state = HFC_inb_nodebug(hc, R_E1_RD_STA)
+					& 0x7;
+				schedule_event(dch, FLG_PHCHANGE);
+				if (debug & DEBUG_HFCMULTI_STATE)
+					printk(KERN_DEBUG
+					    "%s: E1 (id=%d) newstate %x\n",
+					    __func__, hc->id, dch->state);
+				if (test_bit(HFC_CHIP_PLXSD, &hc->chip))
+					plxsd_checksync(hc, 0);
+			}
+		}
+		if (r_irq_misc & V_TI_IRQ)
+			handle_timer_irq(hc);
+
+		if (r_irq_misc & V_DTMF_IRQ) {
+			/* -> DTMF IRQ */
+			hfcmulti_dtmf(hc);
+		}
+		/* TODO: REPLACE !!!! 125 us Interrupts are not acceptable  */
+		if (r_irq_misc & V_IRQ_PROC) {
+			/* IRQ every 125us */
+			count++;
+			/* generate 1kHz signal */
+			if (count == 8) {
+				if (hfc_interrupt)
+					hfc_interrupt();
+				count = 0;
+			}
+		}
+
+	}
+	if (status & V_FR_IRQSTA) {
+		/* FIFO IRQ */
+		r_irq_oview = HFC_inb_nodebug(hc, R_IRQ_OVIEW);
+		for (i = 0; i < 8; i++) {
+			if (r_irq_oview & (1 << i))
+				fifo_irq(hc, i);
+		}
+	}
+
+#ifdef IRQ_DEBUG
+	irqsem = 0;
+#endif
+	spin_unlock(&hc->lock);
+	return IRQ_HANDLED;
+
+irq_notforus:
+#ifdef IRQ_DEBUG
+	irqsem = 0;
+#endif
+	spin_unlock(&hc->lock);
+	return IRQ_NONE;
+}
+
+
+/*
+ * timer callback for D-chan busy resolution. Currently no function
+ */
+
+static void
+hfcmulti_dbusy_timer(struct hfc_multi *hc)
+{
+}
+
+
+/*
+ * activate/deactivate hardware for selected channels and mode
+ *
+ * configure B-channel with the given protocol
+ * ch eqals to the HFC-channel (0-31)
+ * ch is the number of channel (0-4,4-7,8-11,12-15,16-19,20-23,24-27,28-31
+ * for S/T, 1-31 for E1)
+ * the hdlc interrupts will be set/unset
+ */
+static int
+mode_hfcmulti(struct hfc_multi *hc, int ch, int protocol, int slot_tx,
+    int bank_tx, int slot_rx, int bank_rx)
+{
+	int flow_tx = 0, flow_rx = 0, routing = 0;
+	int oslot_tx, oslot_rx;
+	int conf;
+
+	if (ch < 0 || ch > 31)
+		return EINVAL;
+	oslot_tx = hc->chan[ch].slot_tx;
+	oslot_rx = hc->chan[ch].slot_rx;
+	conf = hc->chan[ch].conf;
+
+	if (debug & DEBUG_HFCMULTI_MODE)
+		printk(KERN_DEBUG
+		    "%s: card %d channel %d protocol %x slot old=%d new=%d "
+		    "bank new=%d (TX) slot old=%d new=%d bank new=%d (RX)\n",
+		    __func__, hc->id, ch, protocol, oslot_tx, slot_tx,
+		    bank_tx, oslot_rx, slot_rx, bank_rx);
+
+	if (oslot_tx >= 0 && slot_tx != oslot_tx) {
+		/* remove from slot */
+		if (debug & DEBUG_HFCMULTI_MODE)
+			printk(KERN_DEBUG "%s: remove from slot %d (TX)\n",
+			    __func__, oslot_tx);
+		if (hc->slot_owner[oslot_tx<<1] == ch) {
+			HFC_outb(hc, R_SLOT, oslot_tx << 1);
+			HFC_outb(hc, A_SL_CFG, 0);
+			HFC_outb(hc, A_CONF, 0);
+			hc->slot_owner[oslot_tx<<1] = -1;
+		} else {
+			if (debug & DEBUG_HFCMULTI_MODE)
+				printk(KERN_DEBUG
+				    "%s: we are not owner of this tx slot "
+				    "anymore, channel %d is.\n",
+				    __func__, hc->slot_owner[oslot_tx<<1]);
+		}
+	}
+
+	if (oslot_rx >= 0 && slot_rx != oslot_rx) {
+		/* remove from slot */
+		if (debug & DEBUG_HFCMULTI_MODE)
+			printk(KERN_DEBUG
+			    "%s: remove from slot %d (RX)\n",
+			    __func__, oslot_rx);
+		if (hc->slot_owner[(oslot_rx << 1) | 1] == ch) {
+			HFC_outb(hc, R_SLOT, (oslot_rx << 1) | V_SL_DIR);
+			HFC_outb(hc, A_SL_CFG, 0);
+			hc->slot_owner[(oslot_rx << 1) | 1] = -1;
+		} else {
+			if (debug & DEBUG_HFCMULTI_MODE)
+				printk(KERN_DEBUG
+				    "%s: we are not owner of this rx slot "
+				    "anymore, channel %d is.\n",
+				    __func__,
+				    hc->slot_owner[(oslot_rx << 1) | 1]);
+		}
+	}
+
+	if (slot_tx < 0) {
+		flow_tx = 0x80; /* FIFO->ST */
+		/* disable pcm slot */
+		hc->chan[ch].slot_tx = -1;
+		hc->chan[ch].bank_tx = 0;
+	} else {
+		/* set pcm slot */
+		if (hc->chan[ch].txpending)
+			flow_tx = 0x80; /* FIFO->ST */
+		else
+			flow_tx = 0xc0; /* PCM->ST */
+		/* put on slot */
+		routing = bank_tx ? 0xc0 : 0x80;
+		if (conf >= 0 || bank_tx > 1)
+			routing = 0x40; /* loop */
+		if (debug & DEBUG_HFCMULTI_MODE)
+			printk(KERN_DEBUG "%s: put channel %d to slot %d bank"
+			    " %d flow %02x routing %02x conf %d (TX)\n",
+			    __func__, ch, slot_tx, bank_tx,
+			    flow_tx, routing, conf);
+		HFC_outb(hc, R_SLOT, slot_tx << 1);
+		HFC_outb(hc, A_SL_CFG, (ch<<1) | routing);
+		HFC_outb(hc, A_CONF, (conf < 0) ? 0 : (conf | V_CONF_SL));
+		hc->slot_owner[slot_tx << 1] = ch;
+		hc->chan[ch].slot_tx = slot_tx;
+		hc->chan[ch].bank_tx = bank_tx;
+	}
+	if (slot_rx < 0) {
+		/* disable pcm slot */
+		flow_rx = 0x80; /* ST->FIFO */
+		hc->chan[ch].slot_rx = -1;
+		hc->chan[ch].bank_rx = 0;
+	} else {
+		/* set pcm slot */
+		if (hc->chan[ch].txpending)
+			flow_rx = 0x80; /* ST->FIFO */
+		else
+			flow_rx = 0xc0; /* ST->(FIFO,PCM) */
+		/* put on slot */
+		routing = bank_rx?0x80:0xc0; /* reversed */
+		if (conf >= 0 || bank_rx > 1)
+			routing = 0x40; /* loop */
+		if (debug & DEBUG_HFCMULTI_MODE)
+			printk(KERN_DEBUG "%s: put channel %d to slot %d bank"
+			    " %d flow %02x routing %02x conf %d (RX)\n",
+			    __func__, ch, slot_rx, bank_rx,
+			    flow_rx, routing, conf);
+		HFC_outb(hc, R_SLOT, (slot_rx<<1) | V_SL_DIR);
+		HFC_outb(hc, A_SL_CFG, (ch<<1) | V_CH_DIR | routing);
+		hc->slot_owner[(slot_rx<<1)|1] = ch;
+		hc->chan[ch].slot_rx = slot_rx;
+		hc->chan[ch].bank_rx = bank_rx;
+	}
+
+	switch (protocol) {
+	case (ISDN_P_NONE):
+		/* disable TX fifo */
+		HFC_outb(hc, R_FIFO, ch << 1);
+		HFC_wait(hc);
+		HFC_outb(hc, A_CON_HDLC, flow_tx | 0x00 | V_IFF);
+		HFC_outb(hc, A_SUBCH_CFG, 0);
+		HFC_outb(hc, A_IRQ_MSK, 0);
+		HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+		HFC_wait(hc);
+		/* disable RX fifo */
+		HFC_outb(hc, R_FIFO, (ch<<1)|1);
+		HFC_wait(hc);
+		HFC_outb(hc, A_CON_HDLC, flow_rx | 0x00);
+		HFC_outb(hc, A_SUBCH_CFG, 0);
+		HFC_outb(hc, A_IRQ_MSK, 0);
+		HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+		HFC_wait(hc);
+		if (hc->chan[ch].bch && hc->type != 1) {
+			hc->hw.a_st_ctrl0[hc->chan[ch].port] &=
+			    ((ch & 0x3) == 0)? ~V_B1_EN: ~V_B2_EN;
+			HFC_outb(hc, R_ST_SEL, hc->chan[ch].port);
+			/* undocumented: delay after R_ST_SEL */
+			udelay(1);
+			HFC_outb(hc, A_ST_CTRL0,
+			    hc->hw.a_st_ctrl0[hc->chan[ch].port]);
+		}
+		if (hc->chan[ch].bch) {
+			test_and_clear_bit(FLG_HDLC, &hc->chan[ch].bch->Flags);
+			test_and_clear_bit(FLG_TRANSPARENT,
+			    &hc->chan[ch].bch->Flags);
+		}
+		break;
+	case (ISDN_P_B_RAW): /* B-channel */
+
+		if (test_bit(HFC_CHIP_B410P, &hc->chip) &&
+		    (hc->chan[ch].slot_rx < 0) &&
+		    (hc->chan[ch].slot_tx < 0)) {
+
+			printk(KERN_DEBUG
+			    "Setting B-channel %d to echo cancelable "
+			    "state on PCM slot %d\n", ch,
+			    ((ch / 4) * 8) + ((ch % 4) * 4) + 1);
+			printk(KERN_DEBUG
+			    "Enabling pass through for channel\n");
+			vpm_out(hc, ch, ((ch / 4) * 8) +
+			    ((ch % 4) * 4) + 1, 0x01);
+			/* rx path */
+			/* S/T -> PCM */
+			HFC_outb(hc, R_FIFO, (ch << 1));
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, 0xc0 | V_HDLC_TRP | V_IFF);
+			HFC_outb(hc, R_SLOT, (((ch / 4) * 8) +
+			    ((ch % 4) * 4) + 1) << 1);
+			HFC_outb(hc, A_SL_CFG, 0x80 | (ch << 1));
+
+			/* PCM -> FIFO */
+			HFC_outb(hc, R_FIFO, 0x20 | (ch << 1) | 1);
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, 0x20 | V_HDLC_TRP | V_IFF);
+			HFC_outb(hc, A_SUBCH_CFG, 0);
+			HFC_outb(hc, A_IRQ_MSK, 0);
+			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait(hc);
+			HFC_outb(hc, R_SLOT, ((((ch / 4) * 8) +
+			    ((ch % 4) * 4) + 1) << 1) | 1);
+			HFC_outb(hc, A_SL_CFG, 0x80 | 0x20 | (ch << 1) | 1);
+
+			/* tx path */
+			/* PCM -> S/T */
+			HFC_outb(hc, R_FIFO, (ch << 1) | 1);
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, 0xc0 | V_HDLC_TRP | V_IFF);
+			HFC_outb(hc, R_SLOT, ((((ch / 4) * 8) +
+			    ((ch % 4) * 4)) << 1) | 1);
+			HFC_outb(hc, A_SL_CFG, 0x80 | 0x40 | (ch << 1) | 1);
+
+			/* FIFO -> PCM */
+			HFC_outb(hc, R_FIFO, 0x20 | (ch << 1));
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, 0x20 | V_HDLC_TRP | V_IFF);
+			HFC_outb(hc, A_SUBCH_CFG, 0);
+			HFC_outb(hc, A_IRQ_MSK, 0);
+			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait(hc);
+			/* tx silence */
+			HFC_outb_nodebug(hc, A_FIFO_DATA0_NOINC, silence);
+			HFC_outb(hc, R_SLOT, (((ch / 4) * 8) +
+			    ((ch % 4) * 4)) << 1);
+			HFC_outb(hc, A_SL_CFG, 0x80 | 0x20 | (ch << 1));
+		} else {
+			/* enable TX fifo */
+			HFC_outb(hc, R_FIFO, ch << 1);
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, flow_tx | 0x00 |
+			    V_HDLC_TRP | V_IFF);
+			HFC_outb(hc, A_SUBCH_CFG, 0);
+			HFC_outb(hc, A_IRQ_MSK, 0);
+			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait(hc);
+			/* tx silence */
+			HFC_outb_nodebug(hc, A_FIFO_DATA0_NOINC, silence);
+			/* enable RX fifo */
+			HFC_outb(hc, R_FIFO, (ch<<1)|1);
+			HFC_wait(hc);
+			HFC_outb(hc, A_CON_HDLC, flow_rx | 0x00 | V_HDLC_TRP);
+			HFC_outb(hc, A_SUBCH_CFG, 0);
+			HFC_outb(hc, A_IRQ_MSK, 0);
+			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait(hc);
+		}
+		if (hc->type != 1) {
+			hc->hw.a_st_ctrl0[hc->chan[ch].port] |=
+			    ((ch & 0x3) == 0) ? V_B1_EN : V_B2_EN;
+			HFC_outb(hc, R_ST_SEL, hc->chan[ch].port);
+			/* undocumented: delay after R_ST_SEL */
+			udelay(1);
+			HFC_outb(hc, A_ST_CTRL0,
+			    hc->hw.a_st_ctrl0[hc->chan[ch].port]);
+		}
+		if (hc->chan[ch].bch)
+			test_and_set_bit(FLG_TRANSPARENT,
+			    &hc->chan[ch].bch->Flags);
+		break;
+	case (ISDN_P_B_HDLC): /* B-channel */
+	case (ISDN_P_TE_S0): /* D-channel */
+	case (ISDN_P_NT_S0):
+	case (ISDN_P_TE_E1):
+	case (ISDN_P_NT_E1):
+		/* enable TX fifo */
+		HFC_outb(hc, R_FIFO, ch<<1);
+		HFC_wait(hc);
+		if (hc->type == 1 || hc->chan[ch].bch) {
+			/* E1 or B-channel */
+			HFC_outb(hc, A_CON_HDLC, flow_tx | 0x04);
+			HFC_outb(hc, A_SUBCH_CFG, 0);
+		} else {
+			/* D-Channel without HDLC fill flags */
+			HFC_outb(hc, A_CON_HDLC, flow_tx | 0x04 | V_IFF);
+			HFC_outb(hc, A_SUBCH_CFG, 2);
+		}
+		HFC_outb(hc, A_IRQ_MSK, V_IRQ);
+		HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+		HFC_wait(hc);
+		/* enable RX fifo */
+		HFC_outb(hc, R_FIFO, (ch<<1)|1);
+		HFC_wait(hc);
+		HFC_outb(hc, A_CON_HDLC, flow_rx | 0x04);
+		if (hc->type == 1 || hc->chan[ch].bch)
+			HFC_outb(hc, A_SUBCH_CFG, 0); /* full 8 bits */
+		else
+			HFC_outb(hc, A_SUBCH_CFG, 2); /* 2 bits dchannel */
+		HFC_outb(hc, A_IRQ_MSK, V_IRQ);
+		HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+		HFC_wait(hc);
+		if (hc->chan[ch].bch) {
+			test_and_set_bit(FLG_HDLC, &hc->chan[ch].bch->Flags);
+			if (hc->type != 1) {
+				hc->hw.a_st_ctrl0[hc->chan[ch].port] |=
+				  ((ch&0x3) == 0) ? V_B1_EN : V_B2_EN;
+				HFC_outb(hc, R_ST_SEL, hc->chan[ch].port);
+				/* undocumented: delay after R_ST_SEL */
+				udelay(1);
+				HFC_outb(hc, A_ST_CTRL0,
+				  hc->hw.a_st_ctrl0[hc->chan[ch].port]);
+			}
+		}
+		break;
+	default:
+		printk(KERN_DEBUG "%s: protocol not known %x\n",
+		    __func__, protocol);
+		hc->chan[ch].protocol = ISDN_P_NONE;
+		return -ENOPROTOOPT;
+	}
+	hc->chan[ch].protocol = protocol;
+	return 0;
+}
+
+
+/*
+ * connect/disconnect PCM
+ */
+
+static void
+hfcmulti_pcm(struct hfc_multi *hc, int ch, int slot_tx, int bank_tx,
+    int slot_rx, int bank_rx)
+{
+	if (slot_rx < 0 || slot_rx < 0 || bank_tx < 0 || bank_rx < 0) {
+		/* disable PCM */
+		mode_hfcmulti(hc, ch, hc->chan[ch].protocol, -1, 0, -1, 0);
+		return;
+	}
+
+	/* enable pcm */
+	mode_hfcmulti(hc, ch, hc->chan[ch].protocol, slot_tx, bank_tx,
+		slot_rx, bank_rx);
+}
+
+/*
+ * set/disable conference
+ */
+
+static void
+hfcmulti_conf(struct hfc_multi *hc, int ch, int num)
+{
+	if (num >= 0 && num <= 7)
+		hc->chan[ch].conf = num;
+	else
+		hc->chan[ch].conf = -1;
+	mode_hfcmulti(hc, ch, hc->chan[ch].protocol, hc->chan[ch].slot_tx,
+	    hc->chan[ch].bank_tx, hc->chan[ch].slot_rx,
+	    hc->chan[ch].bank_rx);
+}
+
+
+/*
+ * set/disable sample loop
+ */
+
+/* NOTE: this function is experimental and therefore disabled */
+
+/*
+ * Layer 1 callback function
+ */
+static int
+hfcm_l1callback(struct dchannel *dch, u_int cmd)
+{
+	struct hfc_multi	*hc = dch->hw;
+	u_long	flags;
+
+	switch (cmd) {
+	case INFO3_P8:
+	case INFO3_P10:
+		break;
+	case HW_RESET_REQ:
+		/* start activation */
+		spin_lock_irqsave(&hc->lock, flags);
+		if (hc->type == 1) {
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG
+				    "%s: HW_RESET_REQ no BRI\n",
+				    __func__);
+		} else {
+			HFC_outb(hc, R_ST_SEL, hc->chan[dch->slot].port);
+			/* undocumented: delay after R_ST_SEL */
+			udelay(1);
+			HFC_outb(hc, A_ST_WR_STATE, V_ST_LD_STA | 3); /* F3 */
+			udelay(6); /* wait at least 5,21us */
+			HFC_outb(hc, A_ST_WR_STATE, 3);
+			HFC_outb(hc, A_ST_WR_STATE, 3 | (V_ST_ACT*3));
+				/* activate */
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
+		l1_event(dch->l1, HW_POWERUP_IND);
+		break;
+	case HW_DEACT_REQ:
+		/* start deactivation */
+		spin_lock_irqsave(&hc->lock, flags);
+		if (hc->type == 1) {
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG
+				    "%s: HW_DEACT_REQ no BRI\n",
+				    __func__);
+		} else {
+			HFC_outb(hc, R_ST_SEL, hc->chan[dch->slot].port);
+			/* undocumented: delay after R_ST_SEL */
+			udelay(1);
+			HFC_outb(hc, A_ST_WR_STATE, V_ST_ACT*2);
+				/* deactivate */
+			if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+				hc->syncronized &=
+				   ~(1 << hc->chan[dch->slot].port);
+				plxsd_checksync(hc, 0);
+			}
+		}
+		skb_queue_purge(&dch->squeue);
+		if (dch->tx_skb) {
+			dev_kfree_skb(dch->tx_skb);
+			dch->tx_skb = NULL;
+		}
+		dch->tx_idx = 0;
+		if (dch->rx_skb) {
+			dev_kfree_skb(dch->rx_skb);
+			dch->rx_skb = NULL;
+		}
+		test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
+		if (test_and_clear_bit(FLG_BUSY_TIMER, &dch->Flags))
+			del_timer(&dch->timer);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case HW_POWERUP_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		if (hc->type == 1) {
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG
+				    "%s: HW_POWERUP_REQ no BRI\n",
+				    __func__);
+		} else {
+			HFC_outb(hc, R_ST_SEL, hc->chan[dch->slot].port);
+			/* undocumented: delay after R_ST_SEL */
+			udelay(1);
+			HFC_outb(hc, A_ST_WR_STATE, 3 | 0x10); /* activate */
+			udelay(6); /* wait at least 5,21us */
+			HFC_outb(hc, A_ST_WR_STATE, 3); /* activate */
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case PH_ACTIVATE_IND:
+		test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, cmd, MISDN_ID_ANY, 0, NULL,
+			GFP_ATOMIC);
+		break;
+	case PH_DEACTIVATE_IND:
+		test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, cmd, MISDN_ID_ANY, 0, NULL,
+			GFP_ATOMIC);
+		break;
+	default:
+		if (dch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "%s: unknown command %x\n",
+			    __func__, cmd);
+		return -1;
+	}
+	return 0;
+}
+
+/*
+ * Layer2 -> Layer 1 Transfer
+ */
+
+static int
+handle_dmsg(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct hfc_multi	*hc = dch->hw;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	int			ret = -EINVAL;
+	unsigned int		id;
+	u_long			flags;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		if (skb->len < 1)
+			break;
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = dchannel_senddata(dch, skb);
+		if (ret > 0) { /* direct TX */
+			id = hh->id; /* skb can be freed */
+			hfcmulti_tx(hc, dch->slot);
+			ret = 0;
+			/* start fifo */
+			HFC_outb(hc, R_FIFO, 0);
+			HFC_wait(hc);
+			spin_unlock_irqrestore(&hc->lock, flags);
+			queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
+		} else
+			spin_unlock_irqrestore(&hc->lock, flags);
+		return ret;
+	case PH_ACTIVATE_REQ:
+		if (dch->dev.D.protocol != ISDN_P_TE_S0) {
+			spin_lock_irqsave(&hc->lock, flags);
+			ret = 0;
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG
+				    "%s: PH_ACTIVATE port %d (0..%d)\n",
+				    __func__, hc->chan[dch->slot].port,
+				    hc->ports-1);
+			/* start activation */
+			if (hc->type == 1) {
+				ph_state_change(dch);
+				if (debug & DEBUG_HFCMULTI_STATE)
+					printk(KERN_DEBUG
+					    "%s: E1 report state %x \n",
+					    __func__, dch->state);
+			} else {
+				HFC_outb(hc, R_ST_SEL,
+				    hc->chan[dch->slot].port);
+				/* undocumented: delay after R_ST_SEL */
+				udelay(1);
+				HFC_outb(hc, A_ST_WR_STATE, V_ST_LD_STA | 1);
+				    /* G1 */
+				udelay(6); /* wait at least 5,21us */
+				HFC_outb(hc, A_ST_WR_STATE, 1);
+				HFC_outb(hc, A_ST_WR_STATE, 1 |
+				    (V_ST_ACT*3)); /* activate */
+				dch->state = 1;
+			}
+			spin_unlock_irqrestore(&hc->lock, flags);
+		} else
+			ret = l1_event(dch->l1, hh->prim);
+		break;
+	case PH_DEACTIVATE_REQ:
+		test_and_clear_bit(FLG_L2_ACTIVATED, &dch->Flags);
+		if (dch->dev.D.protocol != ISDN_P_TE_S0) {
+			spin_lock_irqsave(&hc->lock, flags);
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG
+				    "%s: PH_DEACTIVATE port %d (0..%d)\n",
+				    __func__, hc->chan[dch->slot].port,
+				    hc->ports-1);
+			/* start deactivation */
+			if (hc->type == 1) {
+				if (debug & DEBUG_HFCMULTI_MSG)
+					printk(KERN_DEBUG
+					    "%s: PH_DEACTIVATE no BRI\n",
+					    __func__);
+			} else {
+				HFC_outb(hc, R_ST_SEL,
+				    hc->chan[dch->slot].port);
+				/* undocumented: delay after R_ST_SEL */
+				udelay(1);
+				HFC_outb(hc, A_ST_WR_STATE, V_ST_ACT * 2);
+				    /* deactivate */
+				dch->state = 1;
+			}
+			skb_queue_purge(&dch->squeue);
+			if (dch->tx_skb) {
+				dev_kfree_skb(dch->tx_skb);
+				dch->tx_skb = NULL;
+			}
+			dch->tx_idx = 0;
+			if (dch->rx_skb) {
+				dev_kfree_skb(dch->rx_skb);
+				dch->rx_skb = NULL;
+			}
+			test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
+			if (test_and_clear_bit(FLG_BUSY_TIMER, &dch->Flags))
+				del_timer(&dch->timer);
+#ifdef FIXME
+			if (test_and_clear_bit(FLG_L1_BUSY, &dch->Flags))
+				dchannel_sched_event(&hc->dch, D_CLEARBUSY);
+#endif
+			ret = 0;
+			spin_unlock_irqrestore(&hc->lock, flags);
+		} else
+			ret = l1_event(dch->l1, hh->prim);
+		break;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static void
+deactivate_bchannel(struct bchannel *bch)
+{
+	struct hfc_multi	*hc = bch->hw;
+	u_long			flags;
+
+	spin_lock_irqsave(&hc->lock, flags);
+	if (test_and_clear_bit(FLG_TX_NEXT, &bch->Flags)) {
+		dev_kfree_skb(bch->next_skb);
+		bch->next_skb = NULL;
+	}
+	if (bch->tx_skb) {
+		dev_kfree_skb(bch->tx_skb);
+		bch->tx_skb = NULL;
+	}
+	bch->tx_idx = 0;
+	if (bch->rx_skb) {
+		dev_kfree_skb(bch->rx_skb);
+		bch->rx_skb = NULL;
+	}
+	hc->chan[bch->slot].coeff_count = 0;
+	test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
+	test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
+	hc->chan[bch->slot].rx_off = 0;
+	hc->chan[bch->slot].conf = -1;
+	mode_hfcmulti(hc, bch->slot, ISDN_P_NONE, -1, 0, -1, 0);
+	spin_unlock_irqrestore(&hc->lock, flags);
+}
+
+static int
+handle_bmsg(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct bchannel		*bch = container_of(ch, struct bchannel, ch);
+	struct hfc_multi	*hc = bch->hw;
+	int			ret = -EINVAL;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	unsigned int		id;
+	u_long			flags;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		if (!skb->len)
+			break;
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = bchannel_senddata(bch, skb);
+		if (ret > 0) { /* direct TX */
+			id = hh->id; /* skb can be freed */
+			hfcmulti_tx(hc, bch->slot);
+			ret = 0;
+			/* start fifo */
+			HFC_outb_nodebug(hc, R_FIFO, 0);
+			HFC_wait_nodebug(hc);
+			if (!test_bit(FLG_TRANSPARENT, &bch->Flags)) {
+				spin_unlock_irqrestore(&hc->lock, flags);
+				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
+			} else
+				spin_unlock_irqrestore(&hc->lock, flags);
+		} else
+			spin_unlock_irqrestore(&hc->lock, flags);
+		return ret;
+	case PH_ACTIVATE_REQ:
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: PH_ACTIVATE ch %d (0..32)\n",
+				__func__, bch->slot);
+		spin_lock_irqsave(&hc->lock, flags);
+		/* activate B-channel if not already activated */
+		if (!test_and_set_bit(FLG_ACTIVE, &bch->Flags)) {
+			hc->chan[bch->slot].txpending = 0;
+			ret = mode_hfcmulti(hc, bch->slot,
+				ch->protocol,
+				hc->chan[bch->slot].slot_tx,
+				hc->chan[bch->slot].bank_tx,
+				hc->chan[bch->slot].slot_rx,
+				hc->chan[bch->slot].bank_rx);
+			if (!ret) {
+				if (ch->protocol == ISDN_P_B_RAW && !hc->dtmf
+					&& test_bit(HFC_CHIP_DTMF, &hc->chip)) {
+					/* start decoder */
+					hc->dtmf = 1;
+					if (debug & DEBUG_HFCMULTI_DTMF)
+						printk(KERN_DEBUG
+						    "%s: start dtmf decoder\n",
+							__func__);
+					HFC_outb(hc, R_DTMF, hc->hw.r_dtmf |
+					    V_RST_DTMF);
+				}
+			}
+		} else
+			ret = 0;
+		spin_unlock_irqrestore(&hc->lock, flags);
+		if (!ret)
+			_queue_data(ch, PH_ACTIVATE_IND, MISDN_ID_ANY, 0, NULL,
+				GFP_KERNEL);
+		break;
+	case PH_CONTROL_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		switch (hh->id) {
+		case HFC_SPL_LOOP_ON: /* set sample loop */
+			if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG
+			    "%s: HFC_SPL_LOOP_ON (len = %d)\n",
+			    __func__, skb->len);
+			ret = 0;
+			break;
+		case HFC_SPL_LOOP_OFF: /* set silence */
+			if (debug & DEBUG_HFCMULTI_MSG)
+				printk(KERN_DEBUG "%s: HFC_SPL_LOOP_OFF\n",
+				    __func__);
+			ret = 0;
+			break;
+		default:
+			printk(KERN_ERR
+			     "%s: unknown PH_CONTROL_REQ info %x\n",
+			     __func__, hh->id);
+			ret = -EINVAL;
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case PH_DEACTIVATE_REQ:
+		deactivate_bchannel(bch); /* locked there */
+		_queue_data(ch, PH_DEACTIVATE_IND, MISDN_ID_ANY, 0, NULL,
+			GFP_KERNEL);
+		ret = 0;
+		break;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+/*
+ * bchannel control function
+ */
+static int
+channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
+{
+	int			ret = 0;
+	struct dsp_features	*features =
+		(struct dsp_features *)(*((u_long *)&cq->p1));
+	struct hfc_multi	*hc = bch->hw;
+	int			slot_tx;
+	int			bank_tx;
+	int			slot_rx;
+	int			bank_rx;
+	int			num;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = MISDN_CTRL_HFC_OP | MISDN_CTRL_HW_FEATURES_OP
+			| MISDN_CTRL_RX_OFF;
+		break;
+	case MISDN_CTRL_RX_OFF: /* turn off / on rx stream */
+		hc->chan[bch->slot].rx_off = !!cq->p1;
+		if (!hc->chan[bch->slot].rx_off) {
+			/* reset fifo on rx on */
+			HFC_outb_nodebug(hc, R_FIFO, (bch->slot << 1) | 1);
+			HFC_wait_nodebug(hc);
+			HFC_outb_nodebug(hc, R_INC_RES_FIFO, V_RES_F);
+			HFC_wait_nodebug(hc);
+		}
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: RX_OFF request (nr=%d off=%d)\n",
+			    __func__, bch->nr, hc->chan[bch->slot].rx_off);
+		break;
+	case MISDN_CTRL_HW_FEATURES: /* fill features structure */
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HW_FEATURE request\n",
+			    __func__);
+		/* create confirm */
+		features->hfc_id = hc->id;
+		if (test_bit(HFC_CHIP_DTMF, &hc->chip))
+			features->hfc_dtmf = 1;
+		features->hfc_loops = 0;
+		if (test_bit(HFC_CHIP_B410P, &hc->chip)) {
+			features->hfc_echocanhw = 1;
+		} else {
+			features->pcm_id = hc->pcm;
+			features->pcm_slots = hc->slots;
+			features->pcm_banks = 2;
+		}
+		break;
+	case MISDN_CTRL_HFC_PCM_CONN: /* connect to pcm timeslot (0..N) */
+		slot_tx = cq->p1 & 0xff;
+		bank_tx = cq->p1 >> 8;
+		slot_rx = cq->p2 & 0xff;
+		bank_rx = cq->p2 >> 8;
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG
+			    "%s: HFC_PCM_CONN slot %d bank %d (TX) "
+			    "slot %d bank %d (RX)\n",
+			    __func__, slot_tx, bank_tx,
+			    slot_rx, bank_rx);
+		if (slot_tx < hc->slots && bank_tx <= 2 &&
+		    slot_rx < hc->slots && bank_rx <= 2)
+			hfcmulti_pcm(hc, bch->slot,
+			    slot_tx, bank_tx, slot_rx, bank_rx);
+		else {
+			printk(KERN_WARNING
+			    "%s: HFC_PCM_CONN slot %d bank %d (TX) "
+			    "slot %d bank %d (RX) out of range\n",
+			    __func__, slot_tx, bank_tx,
+			    slot_rx, bank_rx);
+			ret = -EINVAL;
+		}
+		break;
+	case MISDN_CTRL_HFC_PCM_DISC: /* release interface from pcm timeslot */
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HFC_PCM_DISC\n",
+			    __func__);
+		hfcmulti_pcm(hc, bch->slot, -1, 0, -1, 0);
+		break;
+	case MISDN_CTRL_HFC_CONF_JOIN: /* join conference (0..7) */
+		num = cq->p1 & 0xff;
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HFC_CONF_JOIN conf %d\n",
+			    __func__, num);
+		if (num <= 7)
+			hfcmulti_conf(hc, bch->slot, num);
+		else {
+			printk(KERN_WARNING
+			    "%s: HW_CONF_JOIN conf %d out of range\n",
+			    __func__, num);
+			ret = -EINVAL;
+		}
+		break;
+	case MISDN_CTRL_HFC_CONF_SPLIT: /* split conference */
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HFC_CONF_SPLIT\n", __func__);
+		hfcmulti_conf(hc, bch->slot, -1);
+		break;
+	case MISDN_CTRL_HFC_ECHOCAN_ON:
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HFC_ECHOCAN_ON\n", __func__);
+		if (test_bit(HFC_CHIP_B410P, &hc->chip))
+			vpm_echocan_on(hc, bch->slot, cq->p1);
+		else
+			ret = -EINVAL;
+		break;
+
+	case MISDN_CTRL_HFC_ECHOCAN_OFF:
+		if (debug & DEBUG_HFCMULTI_MSG)
+			printk(KERN_DEBUG "%s: HFC_ECHOCAN_OFF\n",
+				__func__);
+		if (test_bit(HFC_CHIP_B410P, &hc->chip))
+			vpm_echocan_off(hc, bch->slot);
+		else
+			ret = -EINVAL;
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n",
+		    __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int
+hfcm_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct bchannel		*bch = container_of(ch, struct bchannel, ch);
+	struct hfc_multi	*hc = bch->hw;
+	int			err = -EINVAL;
+	u_long	flags;
+
+	if (bch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n",
+		    __func__, cmd, arg);
+	switch (cmd) {
+	case CLOSE_CHANNEL:
+		test_and_clear_bit(FLG_OPEN, &bch->Flags);
+		if (test_bit(FLG_ACTIVE, &bch->Flags))
+			deactivate_bchannel(bch); /* locked there */
+		ch->protocol = ISDN_P_NONE;
+		ch->peer = NULL;
+		module_put(THIS_MODULE);
+		err = 0;
+		break;
+	case CONTROL_CHANNEL:
+		spin_lock_irqsave(&hc->lock, flags);
+		err = channel_bctrl(bch, arg);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown prim(%x)\n",
+			__func__, cmd);
+	}
+	return err;
+}
+
+/*
+ * handle D-channel events
+ *
+ * handle state change event
+ */
+static void
+ph_state_change(struct dchannel *dch)
+{
+	struct hfc_multi *hc = dch->hw;
+	int ch, i;
+
+	if (!dch) {
+		printk(KERN_WARNING "%s: ERROR given dch is NULL\n",
+		    __func__);
+		return;
+	}
+	ch = dch->slot;
+
+	if (hc->type == 1) {
+		if (dch->dev.D.protocol == ISDN_P_TE_E1) {
+			if (debug & DEBUG_HFCMULTI_STATE)
+				printk(KERN_DEBUG
+				    "%s: E1 TE (id=%d) newstate %x\n",
+				    __func__, hc->id, dch->state);
+		} else {
+			if (debug & DEBUG_HFCMULTI_STATE)
+				printk(KERN_DEBUG
+				    "%s: E1 NT (id=%d) newstate %x\n",
+				    __func__, hc->id, dch->state);
+		}
+		switch (dch->state) {
+		case (1):
+			if (hc->e1_state != 1) {
+			    for (i = 1; i <= 31; i++) {
+				/* reset fifos on e1 activation */
+				HFC_outb_nodebug(hc, R_FIFO, (i << 1) | 1);
+				HFC_wait_nodebug(hc);
+				HFC_outb_nodebug(hc,
+					R_INC_RES_FIFO, V_RES_F);
+				HFC_wait_nodebug(hc);
+			    }
+			}
+			test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+			_queue_data(&dch->dev.D, PH_ACTIVATE_IND,
+			    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+			break;
+
+		default:
+			if (hc->e1_state != 1)
+				return;
+			test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+			_queue_data(&dch->dev.D, PH_DEACTIVATE_IND,
+			    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+		}
+		hc->e1_state = dch->state;
+	} else {
+		if (dch->dev.D.protocol == ISDN_P_TE_S0) {
+			if (debug & DEBUG_HFCMULTI_STATE)
+				printk(KERN_DEBUG
+				    "%s: S/T TE newstate %x\n",
+				    __func__, dch->state);
+			switch (dch->state) {
+			case (0):
+				l1_event(dch->l1, HW_RESET_IND);
+				break;
+			case (3):
+				l1_event(dch->l1, HW_DEACT_IND);
+				break;
+			case (5):
+			case (8):
+				l1_event(dch->l1, ANYSIGNAL);
+				break;
+			case (6):
+				l1_event(dch->l1, INFO2);
+				break;
+			case (7):
+				l1_event(dch->l1, INFO4_P8);
+				break;
+			}
+		} else {
+			if (debug & DEBUG_HFCMULTI_STATE)
+				printk(KERN_DEBUG "%s: S/T NT newstate %x\n",
+				    __func__, dch->state);
+			switch (dch->state) {
+			case (2):
+				if (hc->chan[ch].nt_timer == 0) {
+					hc->chan[ch].nt_timer = -1;
+					HFC_outb(hc, R_ST_SEL,
+					    hc->chan[ch].port);
+					/* undocumented: delay after R_ST_SEL */
+					udelay(1);
+					HFC_outb(hc, A_ST_WR_STATE, 4 |
+					    V_ST_LD_STA); /* G4 */
+					udelay(6); /* wait at least 5,21us */
+					HFC_outb(hc, A_ST_WR_STATE, 4);
+					dch->state = 4;
+				} else {
+					/* one extra count for the next event */
+					hc->chan[ch].nt_timer =
+					    nt_t1_count[poll_timer] + 1;
+					HFC_outb(hc, R_ST_SEL,
+					    hc->chan[ch].port);
+					/* undocumented: delay after R_ST_SEL */
+					udelay(1);
+					/* allow G2 -> G3 transition */
+					HFC_outb(hc, A_ST_WR_STATE, 2 |
+					    V_SET_G2_G3);
+				}
+				break;
+			case (1):
+				hc->chan[ch].nt_timer = -1;
+				test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+				_queue_data(&dch->dev.D, PH_DEACTIVATE_IND,
+				    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+				break;
+			case (4):
+				hc->chan[ch].nt_timer = -1;
+				break;
+			case (3):
+				hc->chan[ch].nt_timer = -1;
+				test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+				_queue_data(&dch->dev.D, PH_ACTIVATE_IND,
+				    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+				break;
+			}
+		}
+	}
+}
+
+/*
+ * called for card mode init message
+ */
+
+static void
+hfcmulti_initmode(struct dchannel *dch)
+{
+	struct hfc_multi *hc = dch->hw;
+	u_char		a_st_wr_state, r_e1_wr_sta;
+	int		i, pt;
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: entered\n", __func__);
+
+	if (hc->type == 1) {
+		hc->chan[hc->dslot].slot_tx = -1;
+		hc->chan[hc->dslot].slot_rx = -1;
+		hc->chan[hc->dslot].conf = -1;
+		if (hc->dslot) {
+			mode_hfcmulti(hc, hc->dslot, dch->dev.D.protocol,
+				-1, 0, -1, 0);
+			dch->timer.function = (void *) hfcmulti_dbusy_timer;
+			dch->timer.data = (long) dch;
+			init_timer(&dch->timer);
+		}
+		for (i = 1; i <= 31; i++) {
+			if (i == hc->dslot)
+				continue;
+			hc->chan[i].slot_tx = -1;
+			hc->chan[i].slot_rx = -1;
+			hc->chan[i].conf = -1;
+			mode_hfcmulti(hc, i, ISDN_P_NONE, -1, 0, -1, 0);
+		}
+		/* E1 */
+		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dslot].cfg)) {
+			HFC_outb(hc, R_LOS0, 255); /* 2 ms */
+			HFC_outb(hc, R_LOS1, 255); /* 512 ms */
+		}
+		if (test_bit(HFC_CFG_OPTICAL, &hc->chan[hc->dslot].cfg)) {
+			HFC_outb(hc, R_RX0, 0);
+			hc->hw.r_tx0 = 0 | V_OUT_EN;
+		} else {
+			HFC_outb(hc, R_RX0, 1);
+			hc->hw.r_tx0 = 1 | V_OUT_EN;
+		}
+		hc->hw.r_tx1 = V_ATX | V_NTRI;
+		HFC_outb(hc, R_TX0, hc->hw.r_tx0);
+		HFC_outb(hc, R_TX1, hc->hw.r_tx1);
+		HFC_outb(hc, R_TX_FR0, 0x00);
+		HFC_outb(hc, R_TX_FR1, 0xf8);
+
+		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dslot].cfg))
+			HFC_outb(hc, R_TX_FR2, V_TX_MF | V_TX_E | V_NEG_E);
+
+		HFC_outb(hc, R_RX_FR0, V_AUTO_RESYNC | V_AUTO_RECO | 0);
+
+		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dslot].cfg))
+			HFC_outb(hc, R_RX_FR1, V_RX_MF | V_RX_MF_SYNC);
+
+		if (dch->dev.D.protocol == ISDN_P_NT_E1) {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG "%s: E1 port is NT-mode\n",
+				    __func__);
+			r_e1_wr_sta = 0; /* G0 */
+			hc->e1_getclock = 0;
+		} else {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG "%s: E1 port is TE-mode\n",
+				    __func__);
+			r_e1_wr_sta = 0; /* F0 */
+			hc->e1_getclock = 1;
+		}
+		if (test_bit(HFC_CHIP_RX_SYNC, &hc->chip))
+			HFC_outb(hc, R_SYNC_OUT, V_SYNC_E1_RX);
+		else
+			HFC_outb(hc, R_SYNC_OUT, 0);
+		if (test_bit(HFC_CHIP_E1CLOCK_GET, &hc->chip))
+			hc->e1_getclock = 1;
+		if (test_bit(HFC_CHIP_E1CLOCK_PUT, &hc->chip))
+			hc->e1_getclock = 0;
+		if (test_bit(HFC_CHIP_PCM_SLAVE, &hc->chip)) {
+			/* SLAVE (clock master) */
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: E1 port is clock master "
+				    "(clock from PCM)\n", __func__);
+			HFC_outb(hc, R_SYNC_CTRL, V_EXT_CLK_SYNC | V_PCM_SYNC);
+		} else {
+			if (hc->e1_getclock) {
+				/* MASTER (clock slave) */
+				if (debug & DEBUG_HFCMULTI_INIT)
+					printk(KERN_DEBUG
+					    "%s: E1 port is clock slave "
+					    "(clock to PCM)\n", __func__);
+				HFC_outb(hc, R_SYNC_CTRL, V_SYNC_OFFS);
+			} else {
+				/* MASTER (clock master) */
+				if (debug & DEBUG_HFCMULTI_INIT)
+					printk(KERN_DEBUG "%s: E1 port is "
+					    "clock master "
+					    "(clock from QUARTZ)\n",
+					    __func__);
+				HFC_outb(hc, R_SYNC_CTRL, V_EXT_CLK_SYNC |
+				    V_PCM_SYNC | V_JATT_OFF);
+				HFC_outb(hc, R_SYNC_OUT, 0);
+			}
+		}
+		HFC_outb(hc, R_JATT_ATT, 0x9c); /* undoc register */
+		HFC_outb(hc, R_PWM_MD, V_PWM0_MD);
+		HFC_outb(hc, R_PWM0, 0x50);
+		HFC_outb(hc, R_PWM1, 0xff);
+		/* state machine setup */
+		HFC_outb(hc, R_E1_WR_STA, r_e1_wr_sta | V_E1_LD_STA);
+		udelay(6); /* wait at least 5,21us */
+		HFC_outb(hc, R_E1_WR_STA, r_e1_wr_sta);
+		if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			hc->syncronized = 0;
+			plxsd_checksync(hc, 0);
+		}
+	} else {
+		i = dch->slot;
+		hc->chan[i].slot_tx = -1;
+		hc->chan[i].slot_rx = -1;
+		hc->chan[i].conf = -1;
+		mode_hfcmulti(hc, i, dch->dev.D.protocol, -1, 0, -1, 0);
+		dch->timer.function = (void *)hfcmulti_dbusy_timer;
+		dch->timer.data = (long) dch;
+		init_timer(&dch->timer);
+		hc->chan[i - 2].slot_tx = -1;
+		hc->chan[i - 2].slot_rx = -1;
+		hc->chan[i - 2].conf = -1;
+		mode_hfcmulti(hc, i - 2, ISDN_P_NONE, -1, 0, -1, 0);
+		hc->chan[i - 1].slot_tx = -1;
+		hc->chan[i - 1].slot_rx = -1;
+		hc->chan[i - 1].conf = -1;
+		mode_hfcmulti(hc, i - 1, ISDN_P_NONE, -1, 0, -1, 0);
+		/* ST */
+		pt = hc->chan[i].port;
+		/* select interface */
+		HFC_outb(hc, R_ST_SEL, pt);
+		/* undocumented: delay after R_ST_SEL */
+		udelay(1);
+		if (dch->dev.D.protocol == ISDN_P_NT_S0) {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: ST port %d is NT-mode\n",
+				    __func__, pt);
+			/* clock delay */
+			HFC_outb(hc, A_ST_CLK_DLY, clockdelay_nt);
+			a_st_wr_state = 1; /* G1 */
+			hc->hw.a_st_ctrl0[pt] = V_ST_MD;
+		} else {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: ST port %d is TE-mode\n",
+				    __func__, pt);
+			/* clock delay */
+			HFC_outb(hc, A_ST_CLK_DLY, clockdelay_te);
+			a_st_wr_state = 2; /* F2 */
+			hc->hw.a_st_ctrl0[pt] = 0;
+		}
+		if (!test_bit(HFC_CFG_NONCAP_TX, &hc->chan[i].cfg))
+			hc->hw.a_st_ctrl0[pt] |= V_TX_LI;
+		/* line setup */
+		HFC_outb(hc, A_ST_CTRL0,  hc->hw.a_st_ctrl0[pt]);
+		/* disable E-channel */
+		if ((dch->dev.D.protocol == ISDN_P_NT_S0) ||
+		    test_bit(HFC_CFG_DIS_ECHANNEL, &hc->chan[i].cfg))
+			HFC_outb(hc, A_ST_CTRL1, V_E_IGNO);
+		else
+			HFC_outb(hc, A_ST_CTRL1, 0);
+		/* enable B-channel receive */
+		HFC_outb(hc, A_ST_CTRL2,  V_B1_RX_EN | V_B2_RX_EN);
+		/* state machine setup */
+		HFC_outb(hc, A_ST_WR_STATE, a_st_wr_state | V_ST_LD_STA);
+		udelay(6); /* wait at least 5,21us */
+		HFC_outb(hc, A_ST_WR_STATE, a_st_wr_state);
+		hc->hw.r_sci_msk |= 1 << pt;
+		/* state machine interrupts */
+		HFC_outb(hc, R_SCI_MSK, hc->hw.r_sci_msk);
+		/* unset sync on port */
+		if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			hc->syncronized &=
+			   ~(1 << hc->chan[dch->slot].port);
+			plxsd_checksync(hc, 0);
+		}
+	}
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk("%s: done\n", __func__);
+}
+
+
+static int
+open_dchannel(struct hfc_multi *hc, struct dchannel *dch,
+    struct channel_req *rq)
+{
+	int	err = 0;
+	u_long	flags;
+
+	if (debug & DEBUG_HW_OPEN)
+		printk(KERN_DEBUG "%s: dev(%d) open from %p\n", __func__,
+		    dch->dev.id, __builtin_return_address(0));
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	if ((dch->dev.D.protocol != ISDN_P_NONE) &&
+	    (dch->dev.D.protocol != rq->protocol)) {
+	    if (debug & DEBUG_HFCMULTI_MODE)
+		printk(KERN_WARNING "%s: change protocol %x to %x\n",
+		    __func__, dch->dev.D.protocol, rq->protocol);
+	}
+	if ((dch->dev.D.protocol == ISDN_P_TE_S0)
+	 && (rq->protocol != ISDN_P_TE_S0))
+		l1_event(dch->l1, CLOSE_CHANNEL);
+	if (dch->dev.D.protocol != rq->protocol) {
+		if (rq->protocol == ISDN_P_TE_S0) {
+			err = create_l1(dch, hfcm_l1callback);
+			if (err)
+				return err;
+		}
+		dch->dev.D.protocol = rq->protocol;
+		spin_lock_irqsave(&hc->lock, flags);
+		hfcmulti_initmode(dch);
+		spin_unlock_irqrestore(&hc->lock, flags);
+	}
+
+	if (((rq->protocol == ISDN_P_NT_S0) && (dch->state == 3)) ||
+	    ((rq->protocol == ISDN_P_TE_S0) && (dch->state == 7)) ||
+	    ((rq->protocol == ISDN_P_NT_E1) && (dch->state == 1)) ||
+	    ((rq->protocol == ISDN_P_TE_E1) && (dch->state == 1))) {
+		_queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY,
+		    0, NULL, GFP_KERNEL);
+	}
+	rq->ch = &dch->dev.D;
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+static int
+open_bchannel(struct hfc_multi *hc, struct dchannel *dch,
+    struct channel_req *rq)
+{
+	struct bchannel	*bch;
+	int		ch;
+
+	if (!test_bit(rq->adr.channel, &dch->dev.channelmap[0]))
+		return -EINVAL;
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	if (hc->type == 1)
+		ch = rq->adr.channel;
+	else
+		ch = (rq->adr.channel - 1) + (dch->slot - 2);
+	bch = hc->chan[ch].bch;
+	if (!bch) {
+		printk(KERN_ERR "%s:internal error ch %d has no bch\n",
+		    __func__, ch);
+		return -EINVAL;
+	}
+	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
+		return -EBUSY; /* b-channel can be only open once */
+	bch->ch.protocol = rq->protocol;
+	hc->chan[ch].rx_off = 0;
+	rq->ch = &bch->ch;
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+/*
+ * device control function
+ */
+static int
+channel_dctrl(struct dchannel *dch, struct mISDN_ctrl_req *cq)
+{
+	int	ret = 0;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = 0;
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n",
+		    __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int
+hfcm_dctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct hfc_multi	*hc = dch->hw;
+	struct channel_req	*rq;
+	int			err = 0;
+	u_long			flags;
+
+	if (dch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n",
+		    __func__, cmd, arg);
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		rq = arg;
+		switch (rq->protocol) {
+		case ISDN_P_TE_S0:
+		case ISDN_P_NT_S0:
+			if (hc->type == 1) {
+				err = -EINVAL;
+				break;
+			}
+			err = open_dchannel(hc, dch, rq); /* locked there */
+			break;
+		case ISDN_P_TE_E1:
+		case ISDN_P_NT_E1:
+			if (hc->type != 1) {
+				err = -EINVAL;
+				break;
+			}
+			err = open_dchannel(hc, dch, rq); /* locked there */
+			break;
+		default:
+			spin_lock_irqsave(&hc->lock, flags);
+			err = open_bchannel(hc, dch, rq);
+			spin_unlock_irqrestore(&hc->lock, flags);
+		}
+		break;
+	case CLOSE_CHANNEL:
+		if (debug & DEBUG_HW_OPEN)
+			printk(KERN_DEBUG "%s: dev(%d) close from %p\n",
+			    __func__, dch->dev.id,
+			    __builtin_return_address(0));
+		module_put(THIS_MODULE);
+		break;
+	case CONTROL_CHANNEL:
+		spin_lock_irqsave(&hc->lock, flags);
+		err = channel_dctrl(dch, arg);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	default:
+		if (dch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "%s: unknown command %x\n",
+			    __func__, cmd);
+		err = -EINVAL;
+	}
+	return err;
+}
+
+/*
+ * initialize the card
+ */
+
+/*
+ * start timer irq, wait some time and check if we have interrupts.
+ * if not, reset chip and try again.
+ */
+static int
+init_card(struct hfc_multi *hc)
+{
+	int	err = -EIO;
+	u_long	flags;
+	u_short	*plx_acc;
+	u_long	plx_flags;
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: entered\n", __func__);
+
+	spin_lock_irqsave(&hc->lock, flags);
+	/* set interrupts but leave global interrupt disabled */
+	hc->hw.r_irq_ctrl = V_FIFO_IRQ;
+	disable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+
+	if (request_irq(hc->pci_dev->irq, hfcmulti_interrupt, IRQF_SHARED,
+	    "HFC-multi", hc)) {
+		printk(KERN_WARNING "mISDN: Could not get interrupt %d.\n",
+		    hc->pci_dev->irq);
+		return -EIO;
+	}
+	hc->irq = hc->pci_dev->irq;
+
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc = (u_short *)(hc->plx_membase+PLX_INTCSR);
+		writew((PLX_INTCSR_PCIINT_ENABLE | PLX_INTCSR_LINTI1_ENABLE),
+			plx_acc); /* enable PCI & LINT1 irq */
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+	}
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: IRQ %d count %d\n",
+		    __func__, hc->irq, hc->irqcnt);
+	err = init_chip(hc);
+	if (err)
+		goto error;
+	/*
+	 * Finally enable IRQ output
+	 * this is only allowed, if an IRQ routine is allready
+	 * established for this HFC, so don't do that earlier
+	 */
+	spin_lock_irqsave(&hc->lock, flags);
+	enable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	/* printk(KERN_DEBUG "no master irq set!!!\n"); */
+	set_current_state(TASK_UNINTERRUPTIBLE);
+	schedule_timeout((100*HZ)/1000); /* Timeout 100ms */
+	/* turn IRQ off until chip is completely initialized */
+	spin_lock_irqsave(&hc->lock, flags);
+	disable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: IRQ %d count %d\n",
+		    __func__, hc->irq, hc->irqcnt);
+	if (hc->irqcnt) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: done\n", __func__);
+
+		return 0;
+	}
+	if (test_bit(HFC_CHIP_PCM_SLAVE, &hc->chip)) {
+		printk(KERN_INFO "ignoring missing interrupts\n");
+		return 0;
+	}
+
+	printk(KERN_ERR "HFC PCI: IRQ(%d) getting no interrupts during init.\n",
+		hc->irq);
+
+	err = -EIO;
+
+error:
+	if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+		spin_lock_irqsave(&plx_lock, plx_flags);
+		plx_acc = (u_short *)(hc->plx_membase+PLX_INTCSR);
+		writew(0x00, plx_acc); /*disable IRQs*/
+		spin_unlock_irqrestore(&plx_lock, plx_flags);
+	}
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_WARNING "%s: free irq %d\n", __func__, hc->irq);
+	if (hc->irq) {
+		free_irq(hc->irq, hc);
+		hc->irq = 0;
+	}
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: done (err=%d)\n", __func__, err);
+	return err;
+}
+
+/*
+ * find pci device and set it up
+ */
+
+static int
+setup_pci(struct hfc_multi *hc, struct pci_dev *pdev,
+		const struct pci_device_id *ent)
+{
+	struct hm_map	*m = (struct hm_map *)ent->driver_data;
+
+	printk(KERN_INFO
+	    "HFC-multi: card manufacturer: '%s' card name: '%s' clock: %s\n",
+	    m->vendor_name, m->card_name, m->clock2 ? "double" : "normal");
+
+	hc->pci_dev = pdev;
+	if (m->clock2)
+		test_and_set_bit(HFC_CHIP_CLOCK2, &hc->chip);
+
+	if (ent->device == 0xB410) {
+		test_and_set_bit(HFC_CHIP_B410P, &hc->chip);
+		test_and_set_bit(HFC_CHIP_PCM_MASTER, &hc->chip);
+		test_and_clear_bit(HFC_CHIP_PCM_SLAVE, &hc->chip);
+		hc->slots = 32;
+	}
+
+	if (hc->pci_dev->irq <= 0) {
+		printk(KERN_WARNING "HFC-multi: No IRQ for PCI card found.\n");
+		return -EIO;
+	}
+	if (pci_enable_device(hc->pci_dev)) {
+		printk(KERN_WARNING "HFC-multi: Error enabling PCI card.\n");
+		return -EIO;
+	}
+	hc->leds = m->leds;
+	hc->ledstate = 0xAFFEAFFE;
+	hc->opticalsupport = m->opticalsupport;
+
+	/* set memory access methods */
+	if (m->io_mode) /* use mode from card config */
+		hc->io_mode = m->io_mode;
+	switch (hc->io_mode) {
+	case HFC_IO_MODE_PLXSD:
+		test_and_set_bit(HFC_CHIP_PLXSD, &hc->chip);
+		hc->slots = 128; /* required */
+		/* fall through */
+	case HFC_IO_MODE_PCIMEM:
+		hc->HFC_outb = HFC_outb_pcimem;
+		hc->HFC_inb = HFC_inb_pcimem;
+		hc->HFC_inw = HFC_inw_pcimem;
+		hc->HFC_wait = HFC_wait_pcimem;
+		hc->read_fifo = read_fifo_pcimem;
+		hc->write_fifo = write_fifo_pcimem;
+		break;
+	case HFC_IO_MODE_REGIO:
+		hc->HFC_outb = HFC_outb_regio;
+		hc->HFC_inb = HFC_inb_regio;
+		hc->HFC_inw = HFC_inw_regio;
+		hc->HFC_wait = HFC_wait_regio;
+		hc->read_fifo = read_fifo_regio;
+		hc->write_fifo = write_fifo_regio;
+		break;
+	default:
+		printk(KERN_WARNING "HFC-multi: Invalid IO mode.\n");
+		pci_disable_device(hc->pci_dev);
+		return -EIO;
+	}
+	hc->HFC_outb_nodebug = hc->HFC_outb;
+	hc->HFC_inb_nodebug = hc->HFC_inb;
+	hc->HFC_inw_nodebug = hc->HFC_inw;
+	hc->HFC_wait_nodebug = hc->HFC_wait;
+#ifdef HFC_REGISTER_DEBUG
+	hc->HFC_outb = HFC_outb_debug;
+	hc->HFC_inb = HFC_inb_debug;
+	hc->HFC_inw = HFC_inw_debug;
+	hc->HFC_wait = HFC_wait_debug;
+#endif
+	hc->pci_iobase = 0;
+	hc->pci_membase = NULL;
+	hc->plx_membase = NULL;
+
+	switch (hc->io_mode) {
+	case HFC_IO_MODE_PLXSD:
+		hc->plx_origmembase =  hc->pci_dev->resource[0].start;
+		/* MEMBASE 1 is PLX PCI Bridge */
+
+		if (!hc->plx_origmembase) {
+			printk(KERN_WARNING
+			  "HFC-multi: No IO-Memory for PCI PLX bridge found\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		hc->plx_membase = ioremap(hc->plx_origmembase, 0x80);
+		if (!hc->plx_membase) {
+			printk(KERN_WARNING
+			    "HFC-multi: failed to remap plx address space. "
+			    "(internal error)\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+		printk(KERN_INFO
+		    "HFC-multi: plx_membase:%#lx plx_origmembase:%#lx\n",
+		    (u_long)hc->plx_membase, hc->plx_origmembase);
+
+		hc->pci_origmembase =  hc->pci_dev->resource[2].start;
+		    /* MEMBASE 1 is PLX PCI Bridge */
+		if (!hc->pci_origmembase) {
+			printk(KERN_WARNING
+			    "HFC-multi: No IO-Memory for PCI card found\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		hc->pci_membase = ioremap(hc->pci_origmembase, 0x400);
+		if (!hc->pci_membase) {
+			printk(KERN_WARNING "HFC-multi: failed to remap io "
+			    "address space. (internal error)\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		printk(KERN_INFO
+		    "card %d: defined at MEMBASE %#lx (%#lx) IRQ %d HZ %d "
+		    "leds-type %d\n",
+		    hc->id, (u_long)hc->pci_membase, hc->pci_origmembase,
+		    hc->pci_dev->irq, HZ, hc->leds);
+		pci_write_config_word(hc->pci_dev, PCI_COMMAND, PCI_ENA_MEMIO);
+		break;
+	case HFC_IO_MODE_PCIMEM:
+		hc->pci_origmembase = hc->pci_dev->resource[1].start;
+		if (!hc->pci_origmembase) {
+			printk(KERN_WARNING
+			    "HFC-multi: No IO-Memory for PCI card found\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		hc->pci_membase = ioremap(hc->pci_origmembase, 256);
+		if (!hc->pci_membase) {
+			printk(KERN_WARNING
+			    "HFC-multi: failed to remap io address space. "
+			    "(internal error)\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+		printk(KERN_INFO "card %d: defined at MEMBASE %#lx (%#lx) IRQ %d "
+		    "HZ %d leds-type %d\n", hc->id, (u_long)hc->pci_membase,
+		    hc->pci_origmembase, hc->pci_dev->irq, HZ, hc->leds);
+		pci_write_config_word(hc->pci_dev, PCI_COMMAND, PCI_ENA_MEMIO);
+		break;
+	case HFC_IO_MODE_REGIO:
+		hc->pci_iobase = (u_int) hc->pci_dev->resource[0].start;
+		if (!hc->pci_iobase) {
+			printk(KERN_WARNING
+				"HFC-multi: No IO for PCI card found\n");
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		if (!request_region(hc->pci_iobase, 8, "hfcmulti")) {
+			printk(KERN_WARNING "HFC-multi: failed to request "
+			    "address space at 0x%08lx (internal error)\n",
+			    hc->pci_iobase);
+			pci_disable_device(hc->pci_dev);
+			return -EIO;
+		}
+
+		printk(KERN_INFO
+		    "%s %s: defined at IOBASE %#x IRQ %d HZ %d leds-type %d\n",
+		    m->vendor_name, m->card_name, (u_int) hc->pci_iobase,
+		    hc->pci_dev->irq, HZ, hc->leds);
+		pci_write_config_word(hc->pci_dev, PCI_COMMAND, PCI_ENA_REGIO);
+		break;
+	default:
+		printk(KERN_WARNING "HFC-multi: Invalid IO mode.\n");
+		pci_disable_device(hc->pci_dev);
+		return -EIO;
+	}
+
+	pci_set_drvdata(hc->pci_dev, hc);
+
+	/* At this point the needed PCI config is done */
+	/* fifos are still not enabled */
+	return 0;
+}
+
+
+/*
+ * remove port
+ */
+
+static void
+release_port(struct hfc_multi *hc, struct dchannel *dch)
+{
+	int	pt, ci, i = 0;
+	u_long	flags;
+	struct bchannel *pb;
+
+	ci = dch->slot;
+	pt = hc->chan[ci].port;
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: entered for port %d\n",
+			__func__, pt + 1);
+
+	if (pt >= hc->ports) {
+		printk(KERN_WARNING "%s: ERROR port out of range (%d).\n",
+		     __func__, pt + 1);
+		return;
+	}
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: releasing port=%d\n",
+		    __func__, pt + 1);
+
+	if (dch->dev.D.protocol == ISDN_P_TE_S0)
+		l1_event(dch->l1, CLOSE_CHANNEL);
+
+	hc->chan[ci].dch = NULL;
+
+	if (hc->created[pt]) {
+		hc->created[pt] = 0;
+		mISDN_unregister_device(&dch->dev);
+	}
+
+	spin_lock_irqsave(&hc->lock, flags);
+
+	if (dch->timer.function) {
+		del_timer(&dch->timer);
+		dch->timer.function = NULL;
+	}
+
+	if (hc->type == 1) { /* E1 */
+		/* remove sync */
+		if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			hc->syncronized = 0;
+			plxsd_checksync(hc, 1);
+		}
+		/* free channels */
+		for (i = 0; i <= 31; i++) {
+			if (hc->chan[i].bch) {
+				if (debug & DEBUG_HFCMULTI_INIT)
+					printk(KERN_DEBUG
+					    "%s: free port %d channel %d\n",
+					    __func__, hc->chan[i].port+1, i);
+				pb = hc->chan[i].bch;
+				hc->chan[i].bch = NULL;
+				spin_unlock_irqrestore(&hc->lock, flags);
+				mISDN_freebchannel(pb);
+				kfree(pb);
+				kfree(hc->chan[i].coeff);
+				spin_lock_irqsave(&hc->lock, flags);
+			}
+		}
+	} else {
+		/* remove sync */
+		if (test_bit(HFC_CHIP_PLXSD, &hc->chip)) {
+			hc->syncronized &=
+			   ~(1 << hc->chan[ci].port);
+			plxsd_checksync(hc, 1);
+		}
+		/* free channels */
+		if (hc->chan[ci - 2].bch) {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: free port %d channel %d\n",
+				    __func__, hc->chan[ci - 2].port+1,
+				    ci - 2);
+			pb = hc->chan[ci - 2].bch;
+			hc->chan[ci - 2].bch = NULL;
+			spin_unlock_irqrestore(&hc->lock, flags);
+			mISDN_freebchannel(pb);
+			kfree(pb);
+			kfree(hc->chan[ci - 2].coeff);
+			spin_lock_irqsave(&hc->lock, flags);
+		}
+		if (hc->chan[ci - 1].bch) {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: free port %d channel %d\n",
+				    __func__, hc->chan[ci - 1].port+1,
+				    ci - 1);
+			pb = hc->chan[ci - 1].bch;
+			hc->chan[ci - 1].bch = NULL;
+			spin_unlock_irqrestore(&hc->lock, flags);
+			mISDN_freebchannel(pb);
+			kfree(pb);
+			kfree(hc->chan[ci - 1].coeff);
+			spin_lock_irqsave(&hc->lock, flags);
+		}
+	}
+
+	spin_unlock_irqrestore(&hc->lock, flags);
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: free port %d channel D\n", __func__, pt);
+	mISDN_freedchannel(dch);
+	kfree(dch);
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: done!\n", __func__);
+}
+
+static void
+release_card(struct hfc_multi *hc)
+{
+	u_long	flags;
+	int	ch;
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_WARNING "%s: release card (%d) entered\n",
+		    __func__, hc->id);
+
+	spin_lock_irqsave(&hc->lock, flags);
+	disable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+
+	udelay(1000);
+
+	/* dimm leds */
+	if (hc->leds)
+		hfcmulti_leds(hc);
+
+	/* disable D-channels & B-channels */
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: disable all channels (d and b)\n",
+		    __func__);
+	for (ch = 0; ch <= 31; ch++) {
+		if (hc->chan[ch].dch)
+			release_port(hc, hc->chan[ch].dch);
+	}
+
+	/* release hardware & irq */
+	if (hc->irq) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_WARNING "%s: free irq %d\n",
+			    __func__, hc->irq);
+		free_irq(hc->irq, hc);
+		hc->irq = 0;
+
+	}
+	release_io_hfcmulti(hc);
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_WARNING "%s: remove instance from list\n",
+		     __func__);
+	list_del(&hc->list);
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_WARNING "%s: delete instance\n", __func__);
+	if (hc == syncmaster)
+		syncmaster = NULL;
+	kfree(hc);
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_WARNING "%s: card successfully removed\n",
+		    __func__);
+}
+
+static int
+init_e1_port(struct hfc_multi *hc, struct hm_map *m)
+{
+	struct dchannel	*dch;
+	struct bchannel	*bch;
+	int		ch, ret = 0;
+	char		name[MISDN_MAX_IDLEN];
+
+	dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL);
+	if (!dch)
+		return -ENOMEM;
+	dch->debug = debug;
+	mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, ph_state_change);
+	dch->hw = hc;
+	dch->dev.Dprotocols = (1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1);
+	dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
+	    (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
+	dch->dev.D.send = handle_dmsg;
+	dch->dev.D.ctrl = hfcm_dctrl;
+	dch->dev.nrbchan = (hc->dslot)?30:31;
+	dch->slot = hc->dslot;
+	hc->chan[hc->dslot].dch = dch;
+	hc->chan[hc->dslot].port = 0;
+	hc->chan[hc->dslot].nt_timer = -1;
+	for (ch = 1; ch <= 31; ch++) {
+		if (ch == hc->dslot) /* skip dchannel */
+			continue;
+		bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL);
+		if (!bch) {
+			printk(KERN_ERR "%s: no memory for bchannel\n",
+			    __func__);
+			ret = -ENOMEM;
+			goto free_chan;
+		}
+		hc->chan[ch].coeff = kzalloc(512, GFP_KERNEL);
+		if (!hc->chan[ch].coeff) {
+			printk(KERN_ERR "%s: no memory for coeffs\n",
+			    __func__);
+			ret = -ENOMEM;
+			goto free_chan;
+		}
+		bch->nr = ch;
+		bch->slot = ch;
+		bch->debug = debug;
+		mISDN_initbchannel(bch, MAX_DATA_MEM);
+		bch->hw = hc;
+		bch->ch.send = handle_bmsg;
+		bch->ch.ctrl = hfcm_bctrl;
+		bch->ch.nr = ch;
+		list_add(&bch->ch.list, &dch->dev.bchannels);
+		hc->chan[ch].bch = bch;
+		hc->chan[ch].port = 0;
+		test_and_set_bit(bch->nr, &dch->dev.channelmap[0]);
+	}
+	/* set optical line type */
+	if (port[Port_cnt] & 0x001) {
+		if (!m->opticalsupport)  {
+			printk(KERN_INFO
+			    "This board has no optical "
+			    "support\n");
+		} else {
+			if (debug & DEBUG_HFCMULTI_INIT)
+				printk(KERN_DEBUG
+				    "%s: PORT set optical "
+				    "interfacs: card(%d) "
+				    "port(%d)\n",
+				    __func__,
+				    HFC_cnt + 1, 1);
+			test_and_set_bit(HFC_CFG_OPTICAL,
+			    &hc->chan[hc->dslot].cfg);
+		}
+	}
+	/* set LOS report */
+	if (port[Port_cnt] & 0x004) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT set "
+			    "LOS report: card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CFG_REPORT_LOS,
+		    &hc->chan[hc->dslot].cfg);
+	}
+	/* set AIS report */
+	if (port[Port_cnt] & 0x008) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT set "
+			    "AIS report: card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CFG_REPORT_AIS,
+		    &hc->chan[hc->dslot].cfg);
+	}
+	/* set SLIP report */
+	if (port[Port_cnt] & 0x010) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PORT set SLIP report: "
+			    "card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CFG_REPORT_SLIP,
+		    &hc->chan[hc->dslot].cfg);
+	}
+	/* set RDI report */
+	if (port[Port_cnt] & 0x020) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PORT set RDI report: "
+			    "card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CFG_REPORT_RDI,
+		    &hc->chan[hc->dslot].cfg);
+	}
+	/* set CRC-4 Mode */
+	if (!(port[Port_cnt] & 0x100)) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT turn on CRC4 report:"
+				" card(%d) port(%d)\n",
+				__func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CFG_CRC4,
+		    &hc->chan[hc->dslot].cfg);
+	} else {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT turn off CRC4"
+				" report: card(%d) port(%d)\n",
+				__func__, HFC_cnt + 1, 1);
+	}
+	/* set forced clock */
+	if (port[Port_cnt] & 0x0200) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT force getting clock from "
+				"E1: card(%d) port(%d)\n",
+				__func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CHIP_E1CLOCK_GET, &hc->chip);
+	} else
+	if (port[Port_cnt] & 0x0400) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT force putting clock to "
+				"E1: card(%d) port(%d)\n",
+				__func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CHIP_E1CLOCK_PUT, &hc->chip);
+	}
+	/* set JATT PLL */
+	if (port[Port_cnt] & 0x0800) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: PORT disable JATT PLL on "
+				"E1: card(%d) port(%d)\n",
+				__func__, HFC_cnt + 1, 1);
+		test_and_set_bit(HFC_CHIP_RX_SYNC, &hc->chip);
+	}
+	/* set elastic jitter buffer */
+	if (port[Port_cnt] & 0x3000) {
+		hc->chan[hc->dslot].jitter = (port[Port_cnt]>>12) & 0x3;
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PORT set elastic "
+			    "buffer to %d: card(%d) port(%d)\n",
+			    __func__, hc->chan[hc->dslot].jitter,
+			    HFC_cnt + 1, 1);
+	} else
+		hc->chan[hc->dslot].jitter = 2; /* default */
+	snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-e1.%d", HFC_cnt + 1);
+	ret = mISDN_register_device(&dch->dev, name);
+	if (ret)
+		goto free_chan;
+	hc->created[0] = 1;
+	return ret;
+free_chan:
+	release_port(hc, dch);
+	return ret;
+}
+
+static int
+init_multi_port(struct hfc_multi *hc, int pt)
+{
+	struct dchannel	*dch;
+	struct bchannel	*bch;
+	int		ch, i, ret = 0;
+	char		name[MISDN_MAX_IDLEN];
+
+	dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL);
+	if (!dch)
+		return -ENOMEM;
+	dch->debug = debug;
+	mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, ph_state_change);
+	dch->hw = hc;
+	dch->dev.Dprotocols = (1 << ISDN_P_TE_S0) | (1 << ISDN_P_NT_S0);
+	dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
+	    (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
+	dch->dev.D.send = handle_dmsg;
+	dch->dev.D.ctrl = hfcm_dctrl;
+	dch->dev.nrbchan = 2;
+	i = pt << 2;
+	dch->slot = i + 2;
+	hc->chan[i + 2].dch = dch;
+	hc->chan[i + 2].port = pt;
+	hc->chan[i + 2].nt_timer = -1;
+	for (ch = 0; ch < dch->dev.nrbchan; ch++) {
+		bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL);
+		if (!bch) {
+			printk(KERN_ERR "%s: no memory for bchannel\n",
+			    __func__);
+			ret = -ENOMEM;
+			goto free_chan;
+		}
+		hc->chan[i + ch].coeff = kzalloc(512, GFP_KERNEL);
+		if (!hc->chan[i + ch].coeff) {
+			printk(KERN_ERR "%s: no memory for coeffs\n",
+			    __func__);
+			ret = -ENOMEM;
+			goto free_chan;
+		}
+		bch->nr = ch + 1;
+		bch->slot = i + ch;
+		bch->debug = debug;
+		mISDN_initbchannel(bch, MAX_DATA_MEM);
+		bch->hw = hc;
+		bch->ch.send = handle_bmsg;
+		bch->ch.ctrl = hfcm_bctrl;
+		bch->ch.nr = ch + 1;
+		list_add(&bch->ch.list, &dch->dev.bchannels);
+		hc->chan[i + ch].bch = bch;
+		hc->chan[i + ch].port = pt;
+		test_and_set_bit(bch->nr, &dch->dev.channelmap[0]);
+	}
+	/* set master clock */
+	if (port[Port_cnt] & 0x001) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PROTOCOL set master clock: "
+			    "card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, pt + 1);
+		if (dch->dev.D.protocol != ISDN_P_TE_S0) {
+			printk(KERN_ERR "Error: Master clock "
+			    "for port(%d) of card(%d) is only"
+			    " possible with TE-mode\n",
+			    pt + 1, HFC_cnt + 1);
+			ret = -EINVAL;
+			goto free_chan;
+		}
+		if (hc->masterclk >= 0) {
+			printk(KERN_ERR "Error: Master clock "
+			    "for port(%d) of card(%d) already "
+			    "defined for port(%d)\n",
+			    pt + 1, HFC_cnt + 1, hc->masterclk+1);
+			ret = -EINVAL;
+			goto free_chan;
+		}
+		hc->masterclk = pt;
+	}
+	/* set transmitter line to non capacitive */
+	if (port[Port_cnt] & 0x002) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PROTOCOL set non capacitive "
+			    "transmitter: card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, pt + 1);
+		test_and_set_bit(HFC_CFG_NONCAP_TX,
+		    &hc->chan[i + 2].cfg);
+	}
+	/* disable E-channel */
+	if (port[Port_cnt] & 0x004) {
+	if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: PROTOCOL disable E-channel: "
+			    "card(%d) port(%d)\n",
+			    __func__, HFC_cnt + 1, pt + 1);
+		test_and_set_bit(HFC_CFG_DIS_ECHANNEL,
+		    &hc->chan[i + 2].cfg);
+	}
+	snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-%ds.%d/%d",
+		hc->type, HFC_cnt + 1, pt + 1);
+	ret = mISDN_register_device(&dch->dev, name);
+	if (ret)
+		goto free_chan;
+	hc->created[pt] = 1;
+	return ret;
+free_chan:
+	release_port(hc, dch);
+	return ret;
+}
+
+static int
+hfcmulti_init(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct hm_map	*m = (struct hm_map *)ent->driver_data;
+	int		ret_err = 0;
+	int		pt;
+	struct hfc_multi	*hc;
+	u_long		flags;
+	u_char		dips = 0, pmj = 0; /* dip settings, port mode Jumpers */
+
+	if (HFC_cnt >= MAX_CARDS) {
+		printk(KERN_ERR "too many cards (max=%d).\n",
+			MAX_CARDS);
+		return -EINVAL;
+	}
+	if ((type[HFC_cnt] & 0xff) && (type[HFC_cnt] & 0xff) != m->type) {
+		printk(KERN_WARNING "HFC-MULTI: Card '%s:%s' type %d found but "
+		    "type[%d] %d was supplied as module parameter\n",
+		    m->vendor_name, m->card_name, m->type, HFC_cnt,
+		    type[HFC_cnt] & 0xff);
+		printk(KERN_WARNING "HFC-MULTI: Load module without parameters "
+			"first, to see cards and their types.");
+		return -EINVAL;
+	}
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: Registering %s:%s chip type %d (0x%x)\n",
+		    __func__, m->vendor_name, m->card_name, m->type,
+		    type[HFC_cnt]);
+
+	/* allocate card+fifo structure */
+	hc = kzalloc(sizeof(struct hfc_multi), GFP_KERNEL);
+	if (!hc) {
+		printk(KERN_ERR "No kmem for HFC-Multi card\n");
+		return -ENOMEM;
+	}
+	spin_lock_init(&hc->lock);
+	hc->mtyp = m;
+	hc->type =  m->type;
+	hc->ports = m->ports;
+	hc->id = HFC_cnt;
+	hc->pcm = pcm[HFC_cnt];
+	hc->io_mode = iomode[HFC_cnt];
+	if (dslot[HFC_cnt] < 0) {
+		hc->dslot = 0;
+		printk(KERN_INFO "HFC-E1 card has disabled D-channel, but "
+			"31 B-channels\n");
+	} if (dslot[HFC_cnt] > 0 && dslot[HFC_cnt] < 32) {
+		hc->dslot = dslot[HFC_cnt];
+		printk(KERN_INFO "HFC-E1 card has alternating D-channel on "
+			"time slot %d\n", dslot[HFC_cnt]);
+	} else
+		hc->dslot = 16;
+
+	/* set chip specific features */
+	hc->masterclk = -1;
+	if (type[HFC_cnt] & 0x100) {
+		test_and_set_bit(HFC_CHIP_ULAW, &hc->chip);
+		silence = 0xff; /* ulaw silence */
+	} else
+		silence = 0x2a; /* alaw silence */
+	if (!(type[HFC_cnt] & 0x200))
+		test_and_set_bit(HFC_CHIP_DTMF, &hc->chip);
+
+	if (type[HFC_cnt] & 0x800)
+		test_and_set_bit(HFC_CHIP_PCM_SLAVE, &hc->chip);
+	if (type[HFC_cnt] & 0x1000) {
+		test_and_set_bit(HFC_CHIP_PCM_MASTER, &hc->chip);
+		test_and_clear_bit(HFC_CHIP_PCM_SLAVE, &hc->chip);
+	}
+	if (type[HFC_cnt] & 0x4000)
+		test_and_set_bit(HFC_CHIP_EXRAM_128, &hc->chip);
+	if (type[HFC_cnt] & 0x8000)
+		test_and_set_bit(HFC_CHIP_EXRAM_512, &hc->chip);
+	hc->slots = 32;
+	if (type[HFC_cnt] & 0x10000)
+		hc->slots = 64;
+	if (type[HFC_cnt] & 0x20000)
+		hc->slots = 128;
+	if (type[HFC_cnt] & 0x80000) {
+		test_and_set_bit(HFC_CHIP_WATCHDOG, &hc->chip);
+		hc->wdcount = 0;
+		hc->wdbyte = V_GPIO_OUT2;
+		printk(KERN_NOTICE "Watchdog enabled\n");
+	}
+
+	/* setup pci, hc->slots may change due to PLXSD */
+	ret_err = setup_pci(hc, pdev, ent);
+	if (ret_err) {
+		if (hc == syncmaster)
+			syncmaster = NULL;
+		kfree(hc);
+		return ret_err;
+	}
+
+	/* crate channels */
+	for (pt = 0; pt < hc->ports; pt++) {
+		if (Port_cnt >= MAX_PORTS) {
+			printk(KERN_ERR "too many ports (max=%d).\n",
+				MAX_PORTS);
+			ret_err = -EINVAL;
+			goto free_card;
+		}
+		if (hc->type == 1)
+			ret_err = init_e1_port(hc, m);
+		else
+			ret_err = init_multi_port(hc, pt);
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG
+			    "%s: Registering D-channel, card(%d) port(%d)"
+			    "result %d\n",
+			    __func__, HFC_cnt + 1, pt, ret_err);
+
+		if (ret_err) {
+			while (pt) { /* release already registered ports */
+				pt--;
+				release_port(hc, hc->chan[(pt << 2) + 2].dch);
+			}
+			goto free_card;
+		}
+		Port_cnt++;
+	}
+
+	/* disp switches */
+	switch (m->dip_type) {
+	case DIP_4S:
+		/*
+		 * get DIP Setting for beroNet 1S/2S/4S cards
+		 *  check if Port Jumper config matches
+		 * module param 'protocol'
+		 * DIP Setting: (collect GPIO 13/14/15 (R_GPIO_IN1) +
+		 * GPI 19/23 (R_GPI_IN2))
+		 */
+		dips = ((~HFC_inb(hc, R_GPIO_IN1) & 0xE0) >> 5) |
+			((~HFC_inb(hc, R_GPI_IN2) & 0x80) >> 3) |
+			(~HFC_inb(hc, R_GPI_IN2) & 0x08);
+
+		/* Port mode (TE/NT) jumpers */
+		pmj = ((HFC_inb(hc, R_GPI_IN3) >> 4)  & 0xf);
+
+		if (test_bit(HFC_CHIP_B410P, &hc->chip))
+			pmj = ~pmj & 0xf;
+
+		printk(KERN_INFO "%s: %s DIPs(0x%x) jumpers(0x%x)\n",
+			m->vendor_name, m->card_name, dips, pmj);
+		break;
+	case DIP_8S:
+		/*
+		 * get DIP Setting for beroNet 8S0+ cards
+		 *
+		 * enable PCI auxbridge function
+		 */
+		HFC_outb(hc, R_BRG_PCM_CFG, 1 | V_PCM_CLK);
+		/* prepare access to auxport */
+		outw(0x4000, hc->pci_iobase + 4);
+		/*
+		 * some dummy reads are required to
+		 * read valid DIP switch data
+		 */
+		dips = inb(hc->pci_iobase);
+		dips = inb(hc->pci_iobase);
+		dips = inb(hc->pci_iobase);
+		dips = ~inb(hc->pci_iobase) & 0x3F;
+		outw(0x0, hc->pci_iobase + 4);
+		/* disable PCI auxbridge function */
+		HFC_outb(hc, R_BRG_PCM_CFG, V_PCM_CLK);
+		printk(KERN_INFO "%s: %s DIPs(0x%x)\n",
+		    m->vendor_name, m->card_name, dips);
+		break;
+	case DIP_E1:
+		/*
+		 * get DIP Setting for beroNet E1 cards
+		 * DIP Setting: collect GPI 4/5/6/7 (R_GPI_IN0)
+		 */
+		dips = (~HFC_inb(hc, R_GPI_IN0) & 0xF0)>>4;
+		printk(KERN_INFO "%s: %s DIPs(0x%x)\n",
+		    m->vendor_name, m->card_name, dips);
+		break;
+	}
+
+	/* add to list */
+	spin_lock_irqsave(&HFClock, flags);
+	list_add_tail(&hc->list, &HFClist);
+	spin_unlock_irqrestore(&HFClock, flags);
+
+	/* initialize hardware */
+	ret_err = init_card(hc);
+	if (ret_err) {
+		printk(KERN_ERR "init card returns %d\n", ret_err);
+		release_card(hc);
+		return ret_err;
+	}
+
+	/* start IRQ and return */
+	spin_lock_irqsave(&hc->lock, flags);
+	enable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	return 0;
+
+free_card:
+	release_io_hfcmulti(hc);
+	if (hc == syncmaster)
+		syncmaster = NULL;
+	kfree(hc);
+	return ret_err;
+}
+
+static void __devexit hfc_remove_pci(struct pci_dev *pdev)
+{
+	struct hfc_multi	*card = pci_get_drvdata(pdev);
+	u_long			flags;
+
+	if (debug)
+		printk(KERN_INFO "removing hfc_multi card vendor:%x "
+		    "device:%x subvendor:%x subdevice:%x\n",
+		    pdev->vendor, pdev->device,
+		    pdev->subsystem_vendor, pdev->subsystem_device);
+
+	if (card) {
+		spin_lock_irqsave(&HFClock, flags);
+		release_card(card);
+		spin_unlock_irqrestore(&HFClock, flags);
+	}  else {
+		if (debug)
+			printk(KERN_WARNING "%s: drvdata allready removed\n",
+			    __func__);
+	}
+}
+
+#define	VENDOR_CCD	"Cologne Chip AG"
+#define	VENDOR_BN	"beroNet GmbH"
+#define	VENDOR_DIG	"Digium Inc."
+#define VENDOR_JH	"Junghanns.NET GmbH"
+#define VENDOR_PRIM	"PrimuX"
+
+static const struct hm_map hfcm_map[] = {
+/*0*/	{VENDOR_BN, "HFC-1S Card (mini PCI)", 4, 1, 1, 3, 0, DIP_4S, 0},
+/*1*/	{VENDOR_BN, "HFC-2S Card", 4, 2, 1, 3, 0, DIP_4S},
+/*2*/	{VENDOR_BN, "HFC-2S Card (mini PCI)", 4, 2, 1, 3, 0, DIP_4S, 0},
+/*3*/	{VENDOR_BN, "HFC-4S Card", 4, 4, 1, 2, 0, DIP_4S, 0},
+/*4*/	{VENDOR_BN, "HFC-4S Card (mini PCI)", 4, 4, 1, 2, 0, 0, 0},
+/*5*/	{VENDOR_CCD, "HFC-4S Eval (old)", 4, 4, 0, 0, 0, 0, 0},
+/*6*/	{VENDOR_CCD, "HFC-4S IOB4ST", 4, 4, 1, 2, 0, 0, 0},
+/*7*/	{VENDOR_CCD, "HFC-4S", 4, 4, 1, 2, 0, 0, 0},
+/*8*/	{VENDOR_DIG, "HFC-4S Card", 4, 4, 0, 2, 0, 0, HFC_IO_MODE_REGIO},
+/*9*/	{VENDOR_CCD, "HFC-4S Swyx 4xS0 SX2 QuadBri", 4, 4, 1, 2, 0, 0, 0},
+/*10*/	{VENDOR_JH, "HFC-4S (junghanns 2.0)", 4, 4, 1, 2, 0, 0, 0},
+/*11*/	{VENDOR_PRIM, "HFC-2S Primux Card", 4, 2, 0, 0, 0, 0, 0},
+
+/*12*/	{VENDOR_BN, "HFC-8S Card", 8, 8, 1, 0, 0, 0, 0},
+/*13*/	{VENDOR_BN, "HFC-8S Card (+)", 8, 8, 1, 8, 0, DIP_8S,
+		HFC_IO_MODE_REGIO},
+/*14*/	{VENDOR_CCD, "HFC-8S Eval (old)", 8, 8, 0, 0, 0, 0, 0},
+/*15*/	{VENDOR_CCD, "HFC-8S IOB4ST Recording", 8, 8, 1, 0, 0, 0, 0},
+
+/*16*/	{VENDOR_CCD, "HFC-8S IOB8ST", 8, 8, 1, 0, 0, 0, 0},
+/*17*/	{VENDOR_CCD, "HFC-8S", 8, 8, 1, 0, 0, 0, 0},
+/*18*/	{VENDOR_CCD, "HFC-8S", 8, 8, 1, 0, 0, 0, 0},
+
+/*19*/	{VENDOR_BN, "HFC-E1 Card", 1, 1, 0, 1, 0, DIP_E1, 0},
+/*20*/	{VENDOR_BN, "HFC-E1 Card (mini PCI)", 1, 1, 0, 1, 0, 0, 0},
+/*21*/	{VENDOR_BN, "HFC-E1+ Card (Dual)", 1, 1, 0, 1, 0, DIP_E1, 0},
+/*22*/	{VENDOR_BN, "HFC-E1 Card (Dual)", 1, 1, 0, 1, 0, DIP_E1, 0},
+
+/*23*/	{VENDOR_CCD, "HFC-E1 Eval (old)", 1, 1, 0, 0, 0, 0, 0},
+/*24*/	{VENDOR_CCD, "HFC-E1 IOB1E1", 1, 1, 0, 1, 0, 0, 0},
+/*25*/	{VENDOR_CCD, "HFC-E1", 1, 1, 0, 1, 0, 0, 0},
+
+/*26*/	{VENDOR_CCD, "HFC-4S Speech Design", 4, 4, 0, 0, 0, 0,
+		HFC_IO_MODE_PLXSD},
+/*27*/	{VENDOR_CCD, "HFC-E1 Speech Design", 1, 1, 0, 0, 0, 0,
+		HFC_IO_MODE_PLXSD},
+/*28*/	{VENDOR_CCD, "HFC-4S OpenVox", 4, 4, 1, 0, 0, 0, 0},
+/*29*/	{VENDOR_CCD, "HFC-2S OpenVox", 4, 2, 1, 0, 0, 0, 0},
+/*30*/	{VENDOR_CCD, "HFC-8S OpenVox", 8, 8, 1, 0, 0, 0, 0},
+};
+
+#undef H
+#define H(x)	((unsigned long)&hfcm_map[x])
+static struct pci_device_id hfmultipci_ids[] __devinitdata = {
+
+	/* Cards with HFC-4S Chip */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BN1SM, 0, 0, H(0)}, /* BN1S mini PCI */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BN2S, 0, 0, H(1)}, /* BN2S */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BN2SM, 0, 0, H(2)}, /* BN2S mini PCI */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BN4S, 0, 0, H(3)}, /* BN4S */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BN4SM, 0, 0, H(4)}, /* BN4S mini PCI */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_DEVICE_ID_CCD_HFC4S, 0, 0, H(5)}, /* Old Eval */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_IOB4ST, 0, 0, H(6)}, /* IOB4ST */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_HFC4S, 0, 0, H(7)}, /* 4S */
+	{ PCI_VENDOR_ID_DIGIUM, PCI_DEVICE_ID_DIGIUM_HFC4S,
+		PCI_VENDOR_ID_DIGIUM, PCI_DEVICE_ID_DIGIUM_HFC4S, 0, 0, H(8)},
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_SWYX4S, 0, 0, H(9)}, /* 4S Swyx */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_JH4S20, 0, 0, H(10)},
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_PMX2S, 0, 0, H(11)}, /* Primux */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_OV4S, 0, 0, H(28)}, /* OpenVox 4 */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_OV2S, 0, 0, H(29)}, /* OpenVox 2 */
+
+	/* Cards with HFC-8S Chip */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+	PCI_SUBDEVICE_ID_CCD_BN8S, 0, 0, H(12)}, /* BN8S */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+	PCI_SUBDEVICE_ID_CCD_BN8SP, 0, 0, H(13)}, /* BN8S+ */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+	PCI_DEVICE_ID_CCD_HFC8S, 0, 0, H(14)}, /* old Eval */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+	PCI_SUBDEVICE_ID_CCD_IOB8STR, 0, 0, H(15)},
+	    /* IOB8ST Recording */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_IOB8ST, 0, 0, H(16)}, /* IOB8ST  */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_IOB8ST_1, 0, 0, H(17)}, /* IOB8ST  */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_HFC8S, 0, 0, H(18)}, /* 8S */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_OV8S, 0, 0, H(30)}, /* OpenVox 8 */
+
+
+	/* Cards with HFC-E1 Chip */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BNE1, 0, 0, H(19)}, /* BNE1 */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BNE1M, 0, 0, H(20)}, /* BNE1 mini PCI */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BNE1DP, 0, 0, H(21)}, /* BNE1 + (Dual) */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_BNE1D, 0, 0, H(22)}, /* BNE1 (Dual) */
+
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_DEVICE_ID_CCD_HFCE1, 0, 0, H(23)}, /* Old Eval */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_IOB1E1, 0, 0, H(24)}, /* IOB1E1 */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_HFCE1, 0, 0, H(25)}, /* E1 */
+
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_SPD4S, 0, 0, H(26)}, /* PLX PCI Bridge */
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_CCD,
+		PCI_SUBDEVICE_ID_CCD_SPDE1, 0, 0, H(27)}, /* PLX PCI Bridge */
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC4S, PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, 0},
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFC8S, PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, 0},
+	{ PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_HFCE1, PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, 0},
+	{0, }
+};
+#undef H
+
+MODULE_DEVICE_TABLE(pci, hfmultipci_ids);
+
+static int
+hfcmulti_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct hm_map	*m = (struct hm_map *)ent->driver_data;
+	int		ret;
+
+	if (m == NULL) {
+		if (ent->vendor == PCI_VENDOR_ID_CCD)
+			if (ent->device == PCI_DEVICE_ID_CCD_HFC4S ||
+			    ent->device == PCI_DEVICE_ID_CCD_HFC8S ||
+			    ent->device == PCI_DEVICE_ID_CCD_HFCE1)
+				printk(KERN_ERR
+				    "unknown HFC multiport controller "
+				    "(vendor:%x device:%x subvendor:%x "
+				    "subdevice:%x) Please contact the "
+				    "driver maintainer for support.\n",
+				    ent->vendor, ent->device,
+				    ent->subvendor, ent->subdevice);
+		return -ENODEV;
+	}
+	ret = hfcmulti_init(pdev, ent);
+	if (ret)
+		return ret;
+	HFC_cnt++;
+	printk(KERN_INFO "%d devices registered\n", HFC_cnt);
+	return 0;
+}
+
+static struct pci_driver hfcmultipci_driver = {
+	.name		= "hfc_multi",
+	.probe		= hfcmulti_probe,
+	.remove		= __devexit_p(hfc_remove_pci),
+	.id_table	= hfmultipci_ids,
+};
+
+static void __exit
+HFCmulti_cleanup(void)
+{
+	struct hfc_multi *card, *next;
+
+	/* unload interrupt function symbol */
+	if (hfc_interrupt)
+		symbol_put(ztdummy_extern_interrupt);
+	if (register_interrupt)
+		symbol_put(ztdummy_register_interrupt);
+	if (unregister_interrupt) {
+		if (interrupt_registered) {
+			interrupt_registered = 0;
+			unregister_interrupt();
+		}
+		symbol_put(ztdummy_unregister_interrupt);
+	}
+
+	list_for_each_entry_safe(card, next, &HFClist, list)
+		release_card(card);
+	/* get rid of all devices of this driver */
+	pci_unregister_driver(&hfcmultipci_driver);
+}
+
+static int __init
+HFCmulti_init(void)
+{
+	int err;
+
+#ifdef IRQ_DEBUG
+	printk(KERN_ERR "%s: IRQ_DEBUG IS ENABLED!\n", __func__);
+#endif
+
+	spin_lock_init(&HFClock);
+	spin_lock_init(&plx_lock);
+
+	if (debug & DEBUG_HFCMULTI_INIT)
+		printk(KERN_DEBUG "%s: init entered\n", __func__);
+
+#ifdef __BIG_ENDIAN
+#error "not running on big endian machines now"
+#endif
+	hfc_interrupt = symbol_get(ztdummy_extern_interrupt);
+	register_interrupt = symbol_get(ztdummy_register_interrupt);
+	unregister_interrupt = symbol_get(ztdummy_unregister_interrupt);
+	printk(KERN_INFO "mISDN: HFC-multi driver %s\n",
+	    hfcmulti_revision);
+
+	switch (poll) {
+	case 0:
+		poll_timer = 6;
+		poll = 128;
+		break;
+		/*
+		 * wenn dieses break nochmal verschwindet,
+		 * gibt es heisse ohren :-)
+		 * "without the break you will get hot ears ???"
+		 */
+	case 8:
+		poll_timer = 2;
+		break;
+	case 16:
+		poll_timer = 3;
+		break;
+	case 32:
+		poll_timer = 4;
+		break;
+	case 64:
+		poll_timer = 5;
+		break;
+	case 128:
+		poll_timer = 6;
+		break;
+	case 256:
+		poll_timer = 7;
+		break;
+	default:
+		printk(KERN_ERR
+		    "%s: Wrong poll value (%d).\n", __func__, poll);
+		err = -EINVAL;
+		return err;
+
+	}
+
+	err = pci_register_driver(&hfcmultipci_driver);
+	if (err < 0) {
+		printk(KERN_ERR "error registering pci driver: %x\n", err);
+		if (hfc_interrupt)
+			symbol_put(ztdummy_extern_interrupt);
+		if (register_interrupt)
+			symbol_put(ztdummy_register_interrupt);
+		if (unregister_interrupt) {
+			if (interrupt_registered) {
+				interrupt_registered = 0;
+				unregister_interrupt();
+			}
+			symbol_put(ztdummy_unregister_interrupt);
+		}
+		return err;
+	}
+	return 0;
+}
+
+
+module_init(HFCmulti_init);
+module_exit(HFCmulti_cleanup);
diff --git a/drivers/isdn/hardware/mISDN/hfcpci.c b/drivers/isdn/hardware/mISDN/hfcpci.c
new file mode 100644
index 00000000000..917968530e1
--- /dev/null
+++ b/drivers/isdn/hardware/mISDN/hfcpci.c
@@ -0,0 +1,2256 @@
+/*
+ *
+ * hfcpci.c     low level driver for CCD's hfc-pci based cards
+ *
+ * Author     Werner Cornelius (werner@isdn4linux.de)
+ *            based on existing driver for CCD hfc ISA cards
+ *            type approval valid for HFC-S PCI A based card
+ *
+ * Copyright 1999  by Werner Cornelius (werner@isdn-development.de)
+ * Copyright 2008  by Karsten Keil <kkeil@novell.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/pci.h>
+#include <linux/delay.h>
+#include <linux/mISDNhw.h>
+
+#include "hfc_pci.h"
+
+static const char *hfcpci_revision = "2.0";
+
+#define MAX_CARDS	8
+static int HFC_cnt;
+static uint debug;
+
+MODULE_AUTHOR("Karsten Keil");
+MODULE_LICENSE("GPL");
+module_param(debug, uint, 0);
+
+static LIST_HEAD(HFClist);
+DEFINE_RWLOCK(HFClock);
+
+enum {
+	HFC_CCD_2BD0,
+	HFC_CCD_B000,
+	HFC_CCD_B006,
+	HFC_CCD_B007,
+	HFC_CCD_B008,
+	HFC_CCD_B009,
+	HFC_CCD_B00A,
+	HFC_CCD_B00B,
+	HFC_CCD_B00C,
+	HFC_CCD_B100,
+	HFC_CCD_B700,
+	HFC_CCD_B701,
+	HFC_ASUS_0675,
+	HFC_BERKOM_A1T,
+	HFC_BERKOM_TCONCEPT,
+	HFC_ANIGMA_MC145575,
+	HFC_ZOLTRIX_2BD0,
+	HFC_DIGI_DF_M_IOM2_E,
+	HFC_DIGI_DF_M_E,
+	HFC_DIGI_DF_M_IOM2_A,
+	HFC_DIGI_DF_M_A,
+	HFC_ABOCOM_2BD1,
+	HFC_SITECOM_DC105V2,
+};
+
+struct hfcPCI_hw {
+	unsigned char		cirm;
+	unsigned char		ctmt;
+	unsigned char		clkdel;
+	unsigned char		states;
+	unsigned char		conn;
+	unsigned char		mst_m;
+	unsigned char		int_m1;
+	unsigned char		int_m2;
+	unsigned char		sctrl;
+	unsigned char		sctrl_r;
+	unsigned char		sctrl_e;
+	unsigned char		trm;
+	unsigned char		fifo_en;
+	unsigned char		bswapped;
+	unsigned char		protocol;
+	int			nt_timer;
+	unsigned char		*pci_io; /* start of PCI IO memory */
+	dma_addr_t		dmahandle;
+	void			*fifos; /* FIFO memory */
+	int			last_bfifo_cnt[2];
+	    /* marker saving last b-fifo frame count */
+	struct timer_list	timer;
+};
+
+#define	HFC_CFG_MASTER		1
+#define HFC_CFG_SLAVE		2
+#define	HFC_CFG_PCM		3
+#define HFC_CFG_2HFC		4
+#define HFC_CFG_SLAVEHFC	5
+#define HFC_CFG_NEG_F0		6
+#define HFC_CFG_SW_DD_DU	7
+
+#define FLG_HFC_TIMER_T1	16
+#define FLG_HFC_TIMER_T3	17
+
+#define NT_T1_COUNT	1120	/* number of 3.125ms interrupts (3.5s) */
+#define NT_T3_COUNT	31	/* number of 3.125ms interrupts (97 ms) */
+#define CLKDEL_TE	0x0e	/* CLKDEL in TE mode */
+#define CLKDEL_NT	0x6c	/* CLKDEL in NT mode */
+
+
+struct hfc_pci {
+	struct list_head	list;
+	u_char			subtype;
+	u_char			chanlimit;
+	u_char			initdone;
+	u_long			cfg;
+	u_int			irq;
+	u_int			irqcnt;
+	struct pci_dev		*pdev;
+	struct hfcPCI_hw	hw;
+	spinlock_t		lock;	/* card lock */
+	struct dchannel		dch;
+	struct bchannel		bch[2];
+};
+
+/* Interface functions */
+static void
+enable_hwirq(struct hfc_pci *hc)
+{
+	hc->hw.int_m2 |= HFCPCI_IRQ_ENABLE;
+	Write_hfc(hc, HFCPCI_INT_M2, hc->hw.int_m2);
+}
+
+static void
+disable_hwirq(struct hfc_pci *hc)
+{
+	hc->hw.int_m2 &= ~((u_char)HFCPCI_IRQ_ENABLE);
+	Write_hfc(hc, HFCPCI_INT_M2, hc->hw.int_m2);
+}
+
+/*
+ * free hardware resources used by driver
+ */
+static void
+release_io_hfcpci(struct hfc_pci *hc)
+{
+	/* disable memory mapped ports + busmaster */
+	pci_write_config_word(hc->pdev, PCI_COMMAND, 0);
+	del_timer(&hc->hw.timer);
+	pci_free_consistent(hc->pdev, 0x8000, hc->hw.fifos, hc->hw.dmahandle);
+	iounmap((void *)hc->hw.pci_io);
+}
+
+/*
+ * set mode (NT or TE)
+ */
+static void
+hfcpci_setmode(struct hfc_pci *hc)
+{
+	if (hc->hw.protocol == ISDN_P_NT_S0) {
+		hc->hw.clkdel = CLKDEL_NT;	/* ST-Bit delay for NT-Mode */
+		hc->hw.sctrl |= SCTRL_MODE_NT;	/* NT-MODE */
+		hc->hw.states = 1;		/* G1 */
+	} else {
+		hc->hw.clkdel = CLKDEL_TE;	/* ST-Bit delay for TE-Mode */
+		hc->hw.sctrl &= ~SCTRL_MODE_NT;	/* TE-MODE */
+		hc->hw.states = 2;		/* F2 */
+	}
+	Write_hfc(hc, HFCPCI_CLKDEL, hc->hw.clkdel);
+	Write_hfc(hc, HFCPCI_STATES, HFCPCI_LOAD_STATE | hc->hw.states);
+	udelay(10);
+	Write_hfc(hc, HFCPCI_STATES, hc->hw.states | 0x40); /* Deactivate */
+	Write_hfc(hc, HFCPCI_SCTRL, hc->hw.sctrl);
+}
+
+/*
+ * function called to reset the HFC PCI chip. A complete software reset of chip
+ * and fifos is done.
+ */
+static void
+reset_hfcpci(struct hfc_pci *hc)
+{
+	u_char	val;
+	int	cnt = 0;
+
+	printk(KERN_DEBUG "reset_hfcpci: entered\n");
+	val = Read_hfc(hc, HFCPCI_CHIP_ID);
+	printk(KERN_INFO "HFC_PCI: resetting HFC ChipId(%x)\n", val);
+	/* enable memory mapped ports, disable busmaster */
+	pci_write_config_word(hc->pdev, PCI_COMMAND, PCI_ENA_MEMIO);
+	disable_hwirq(hc);
+	/* enable memory ports + busmaster */
+	pci_write_config_word(hc->pdev, PCI_COMMAND,
+	    PCI_ENA_MEMIO + PCI_ENA_MASTER);
+	val = Read_hfc(hc, HFCPCI_STATUS);
+	printk(KERN_DEBUG "HFC-PCI status(%x) before reset\n", val);
+	hc->hw.cirm = HFCPCI_RESET;	/* Reset On */
+	Write_hfc(hc, HFCPCI_CIRM, hc->hw.cirm);
+	set_current_state(TASK_UNINTERRUPTIBLE);
+	mdelay(10);			/* Timeout 10ms */
+	hc->hw.cirm = 0;		/* Reset Off */
+	Write_hfc(hc, HFCPCI_CIRM, hc->hw.cirm);
+	val = Read_hfc(hc, HFCPCI_STATUS);
+	printk(KERN_DEBUG "HFC-PCI status(%x) after reset\n", val);
+	while (cnt < 50000) { /* max 50000 us */
+		udelay(5);
+		cnt += 5;
+		val = Read_hfc(hc, HFCPCI_STATUS);
+		if (!(val & 2))
+			break;
+	}
+	printk(KERN_DEBUG "HFC-PCI status(%x) after %dus\n", val, cnt);
+
+	hc->hw.fifo_en = 0x30;	/* only D fifos enabled */
+
+	hc->hw.bswapped = 0;	/* no exchange */
+	hc->hw.ctmt = HFCPCI_TIM3_125 | HFCPCI_AUTO_TIMER;
+	hc->hw.trm = HFCPCI_BTRANS_THRESMASK; /* no echo connect , threshold */
+	hc->hw.sctrl = 0x40;	/* set tx_lo mode, error in datasheet ! */
+	hc->hw.sctrl_r = 0;
+	hc->hw.sctrl_e = HFCPCI_AUTO_AWAKE;	/* S/T Auto awake */
+	hc->hw.mst_m = 0;
+	if (test_bit(HFC_CFG_MASTER, &hc->cfg))
+		hc->hw.mst_m |= HFCPCI_MASTER;	/* HFC Master Mode */
+	if (test_bit(HFC_CFG_NEG_F0, &hc->cfg))
+		hc->hw.mst_m |= HFCPCI_F0_NEGATIV;
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	Write_hfc(hc, HFCPCI_TRM, hc->hw.trm);
+	Write_hfc(hc, HFCPCI_SCTRL_E, hc->hw.sctrl_e);
+	Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt);
+
+	hc->hw.int_m1 = HFCPCI_INTS_DTRANS | HFCPCI_INTS_DREC |
+	    HFCPCI_INTS_L1STATE | HFCPCI_INTS_TIMER;
+	Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+
+	/* Clear already pending ints */
+	if (Read_hfc(hc, HFCPCI_INT_S1));
+
+	/* set NT/TE mode */
+	hfcpci_setmode(hc);
+
+	Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+	Write_hfc(hc, HFCPCI_SCTRL_R, hc->hw.sctrl_r);
+
+	/*
+	 * Init GCI/IOM2 in master mode
+	 * Slots 0 and 1 are set for B-chan 1 and 2
+	 * D- and monitor/CI channel are not enabled
+	 * STIO1 is used as output for data, B1+B2 from ST->IOM+HFC
+	 * STIO2 is used as data input, B1+B2 from IOM->ST
+	 * ST B-channel send disabled -> continous 1s
+	 * The IOM slots are always enabled
+	 */
+	if (test_bit(HFC_CFG_PCM, &hc->cfg)) {
+		/* set data flow directions: connect B1,B2: HFC to/from PCM */
+		hc->hw.conn = 0x09;
+	} else {
+		hc->hw.conn = 0x36;	/* set data flow directions */
+		if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg)) {
+			Write_hfc(hc, HFCPCI_B1_SSL, 0xC0);
+			Write_hfc(hc, HFCPCI_B2_SSL, 0xC1);
+			Write_hfc(hc, HFCPCI_B1_RSL, 0xC0);
+			Write_hfc(hc, HFCPCI_B2_RSL, 0xC1);
+		} else {
+			Write_hfc(hc, HFCPCI_B1_SSL, 0x80);
+			Write_hfc(hc, HFCPCI_B2_SSL, 0x81);
+			Write_hfc(hc, HFCPCI_B1_RSL, 0x80);
+			Write_hfc(hc, HFCPCI_B2_RSL, 0x81);
+		}
+	}
+	Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+	val = Read_hfc(hc, HFCPCI_INT_S2);
+}
+
+/*
+ * Timer function called when kernel timer expires
+ */
+static void
+hfcpci_Timer(struct hfc_pci *hc)
+{
+	hc->hw.timer.expires = jiffies + 75;
+	/* WD RESET */
+/*
+ *	WriteReg(hc, HFCD_DATA, HFCD_CTMT, hc->hw.ctmt | 0x80);
+ *	add_timer(&hc->hw.timer);
+ */
+}
+
+
+/*
+ * select a b-channel entry matching and active
+ */
+static struct bchannel *
+Sel_BCS(struct hfc_pci *hc, int channel)
+{
+	if (test_bit(FLG_ACTIVE, &hc->bch[0].Flags) &&
+		(hc->bch[0].nr & channel))
+		return &hc->bch[0];
+	else if (test_bit(FLG_ACTIVE, &hc->bch[1].Flags) &&
+		(hc->bch[1].nr & channel))
+		return &hc->bch[1];
+	else
+		return NULL;
+}
+
+/*
+ * clear the desired B-channel rx fifo
+ */
+static void
+hfcpci_clear_fifo_rx(struct hfc_pci *hc, int fifo)
+{
+	u_char		fifo_state;
+	struct bzfifo	*bzr;
+
+	if (fifo) {
+		bzr = &((union fifo_area *)(hc->hw.fifos))->b_chans.rxbz_b2;
+		fifo_state = hc->hw.fifo_en & HFCPCI_FIFOEN_B2RX;
+	} else {
+		bzr = &((union fifo_area *)(hc->hw.fifos))->b_chans.rxbz_b1;
+		fifo_state = hc->hw.fifo_en & HFCPCI_FIFOEN_B1RX;
+	}
+	if (fifo_state)
+		hc->hw.fifo_en ^= fifo_state;
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	hc->hw.last_bfifo_cnt[fifo] = 0;
+	bzr->f1 = MAX_B_FRAMES;
+	bzr->f2 = bzr->f1;	/* init F pointers to remain constant */
+	bzr->za[MAX_B_FRAMES].z1 = cpu_to_le16(B_FIFO_SIZE + B_SUB_VAL - 1);
+	bzr->za[MAX_B_FRAMES].z2 = cpu_to_le16(
+	    le16_to_cpu(bzr->za[MAX_B_FRAMES].z1));
+	if (fifo_state)
+		hc->hw.fifo_en |= fifo_state;
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+}
+
+/*
+ * clear the desired B-channel tx fifo
+ */
+static void hfcpci_clear_fifo_tx(struct hfc_pci *hc, int fifo)
+{
+	u_char		fifo_state;
+	struct bzfifo	*bzt;
+
+	if (fifo) {
+		bzt = &((union fifo_area *)(hc->hw.fifos))->b_chans.txbz_b2;
+		fifo_state = hc->hw.fifo_en & HFCPCI_FIFOEN_B2TX;
+	} else {
+		bzt = &((union fifo_area *)(hc->hw.fifos))->b_chans.txbz_b1;
+		fifo_state = hc->hw.fifo_en & HFCPCI_FIFOEN_B1TX;
+	}
+	if (fifo_state)
+		hc->hw.fifo_en ^= fifo_state;
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	if (hc->bch[fifo].debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG "hfcpci_clear_fifo_tx%d f1(%x) f2(%x) "
+		    "z1(%x) z2(%x) state(%x)\n",
+		    fifo, bzt->f1, bzt->f2,
+		    le16_to_cpu(bzt->za[MAX_B_FRAMES].z1),
+		    le16_to_cpu(bzt->za[MAX_B_FRAMES].z2),
+		    fifo_state);
+	bzt->f2 = MAX_B_FRAMES;
+	bzt->f1 = bzt->f2;	/* init F pointers to remain constant */
+	bzt->za[MAX_B_FRAMES].z1 = cpu_to_le16(B_FIFO_SIZE + B_SUB_VAL - 1);
+	bzt->za[MAX_B_FRAMES].z2 = cpu_to_le16(
+	    le16_to_cpu(bzt->za[MAX_B_FRAMES].z1 - 1));
+	if (fifo_state)
+		hc->hw.fifo_en |= fifo_state;
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	if (hc->bch[fifo].debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG
+		    "hfcpci_clear_fifo_tx%d f1(%x) f2(%x) z1(%x) z2(%x)\n",
+		    fifo, bzt->f1, bzt->f2,
+		    le16_to_cpu(bzt->za[MAX_B_FRAMES].z1),
+		    le16_to_cpu(bzt->za[MAX_B_FRAMES].z2));
+}
+
+/*
+ * read a complete B-frame out of the buffer
+ */
+static void
+hfcpci_empty_bfifo(struct bchannel *bch, struct bzfifo *bz,
+    u_char *bdata, int count)
+{
+	u_char		*ptr, *ptr1, new_f2;
+	int		total, maxlen, new_z2;
+	struct zt	*zp;
+
+	if ((bch->debug & DEBUG_HW_BCHANNEL) && !(bch->debug & DEBUG_HW_BFIFO))
+		printk(KERN_DEBUG "hfcpci_empty_fifo\n");
+	zp = &bz->za[bz->f2];	/* point to Z-Regs */
+	new_z2 = le16_to_cpu(zp->z2) + count;	/* new position in fifo */
+	if (new_z2 >= (B_FIFO_SIZE + B_SUB_VAL))
+		new_z2 -= B_FIFO_SIZE;	/* buffer wrap */
+	new_f2 = (bz->f2 + 1) & MAX_B_FRAMES;
+	if ((count > MAX_DATA_SIZE + 3) || (count < 4) ||
+	    (*(bdata + (le16_to_cpu(zp->z1) - B_SUB_VAL)))) {
+		if (bch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "hfcpci_empty_fifo: incoming packet "
+			    "invalid length %d or crc\n", count);
+#ifdef ERROR_STATISTIC
+		bch->err_inv++;
+#endif
+		bz->za[new_f2].z2 = cpu_to_le16(new_z2);
+		bz->f2 = new_f2;	/* next buffer */
+	} else {
+		bch->rx_skb = mI_alloc_skb(count - 3, GFP_ATOMIC);
+		if (!bch->rx_skb) {
+			printk(KERN_WARNING "HFCPCI: receive out of memory\n");
+			return;
+		}
+		total = count;
+		count -= 3;
+		ptr = skb_put(bch->rx_skb, count);
+
+		if (le16_to_cpu(zp->z2) + count <= B_FIFO_SIZE + B_SUB_VAL)
+			maxlen = count;		/* complete transfer */
+		else
+			maxlen = B_FIFO_SIZE + B_SUB_VAL -
+			    le16_to_cpu(zp->z2);	/* maximum */
+
+		ptr1 = bdata + (le16_to_cpu(zp->z2) - B_SUB_VAL);
+		    /* start of data */
+		memcpy(ptr, ptr1, maxlen);	/* copy data */
+		count -= maxlen;
+
+		if (count) {	/* rest remaining */
+			ptr += maxlen;
+			ptr1 = bdata;	/* start of buffer */
+			memcpy(ptr, ptr1, count);	/* rest */
+		}
+		bz->za[new_f2].z2 = cpu_to_le16(new_z2);
+		bz->f2 = new_f2;	/* next buffer */
+		recv_Bchannel(bch);
+	}
+}
+
+/*
+ * D-channel receive procedure
+ */
+static int
+receive_dmsg(struct hfc_pci *hc)
+{
+	struct dchannel	*dch = &hc->dch;
+	int		maxlen;
+	int		rcnt, total;
+	int		count = 5;
+	u_char		*ptr, *ptr1;
+	struct dfifo	*df;
+	struct zt	*zp;
+
+	df = &((union fifo_area *)(hc->hw.fifos))->d_chan.d_rx;
+	while (((df->f1 & D_FREG_MASK) != (df->f2 & D_FREG_MASK)) && count--) {
+		zp = &df->za[df->f2 & D_FREG_MASK];
+		rcnt = le16_to_cpu(zp->z1) - le16_to_cpu(zp->z2);
+		if (rcnt < 0)
+			rcnt += D_FIFO_SIZE;
+		rcnt++;
+		if (dch->debug & DEBUG_HW_DCHANNEL)
+			printk(KERN_DEBUG
+			    "hfcpci recd f1(%d) f2(%d) z1(%x) z2(%x) cnt(%d)\n",
+				df->f1, df->f2,
+				le16_to_cpu(zp->z1),
+				le16_to_cpu(zp->z2),
+				rcnt);
+
+		if ((rcnt > MAX_DFRAME_LEN + 3) || (rcnt < 4) ||
+		    (df->data[le16_to_cpu(zp->z1)])) {
+			if (dch->debug & DEBUG_HW)
+				printk(KERN_DEBUG
+				    "empty_fifo hfcpci paket inv. len "
+				    "%d or crc %d\n",
+				    rcnt,
+				    df->data[le16_to_cpu(zp->z1)]);
+#ifdef ERROR_STATISTIC
+			cs->err_rx++;
+#endif
+			df->f2 = ((df->f2 + 1) & MAX_D_FRAMES) |
+			    (MAX_D_FRAMES + 1);	/* next buffer */
+			df->za[df->f2 & D_FREG_MASK].z2 =
+			    cpu_to_le16((zp->z2 + rcnt) & (D_FIFO_SIZE - 1));
+		} else {
+			dch->rx_skb = mI_alloc_skb(rcnt - 3, GFP_ATOMIC);
+			if (!dch->rx_skb) {
+				printk(KERN_WARNING
+				    "HFC-PCI: D receive out of memory\n");
+				break;
+			}
+			total = rcnt;
+			rcnt -= 3;
+			ptr = skb_put(dch->rx_skb, rcnt);
+
+			if (le16_to_cpu(zp->z2) + rcnt <= D_FIFO_SIZE)
+				maxlen = rcnt;	/* complete transfer */
+			else
+				maxlen = D_FIFO_SIZE - le16_to_cpu(zp->z2);
+				    /* maximum */
+
+			ptr1 = df->data + le16_to_cpu(zp->z2);
+			    /* start of data */
+			memcpy(ptr, ptr1, maxlen);	/* copy data */
+			rcnt -= maxlen;
+
+			if (rcnt) {	/* rest remaining */
+				ptr += maxlen;
+				ptr1 = df->data;	/* start of buffer */
+				memcpy(ptr, ptr1, rcnt);	/* rest */
+			}
+			df->f2 = ((df->f2 + 1) & MAX_D_FRAMES) |
+			    (MAX_D_FRAMES + 1);	/* next buffer */
+			df->za[df->f2 & D_FREG_MASK].z2 = cpu_to_le16((
+			    le16_to_cpu(zp->z2) + total) & (D_FIFO_SIZE - 1));
+			recv_Dchannel(dch);
+		}
+	}
+	return 1;
+}
+
+/*
+ * check for transparent receive data and read max one threshold size if avail
+ */
+int
+hfcpci_empty_fifo_trans(struct bchannel *bch, struct bzfifo *bz, u_char *bdata)
+{
+	unsigned short	*z1r, *z2r;
+	int		new_z2, fcnt, maxlen;
+	u_char		*ptr, *ptr1;
+
+	z1r = &bz->za[MAX_B_FRAMES].z1;		/* pointer to z reg */
+	z2r = z1r + 1;
+
+	fcnt = le16_to_cpu(*z1r) - le16_to_cpu(*z2r);
+	if (!fcnt)
+		return 0;	/* no data avail */
+
+	if (fcnt <= 0)
+		fcnt += B_FIFO_SIZE;	/* bytes actually buffered */
+	if (fcnt > HFCPCI_BTRANS_THRESHOLD)
+		fcnt = HFCPCI_BTRANS_THRESHOLD;		/* limit size */
+
+	new_z2 = le16_to_cpu(*z2r) + fcnt;	/* new position in fifo */
+	if (new_z2 >= (B_FIFO_SIZE + B_SUB_VAL))
+		new_z2 -= B_FIFO_SIZE;	/* buffer wrap */
+
+	bch->rx_skb = mI_alloc_skb(fcnt, GFP_ATOMIC);
+	if (bch->rx_skb) {
+		ptr = skb_put(bch->rx_skb, fcnt);
+		if (le16_to_cpu(*z2r) + fcnt <= B_FIFO_SIZE + B_SUB_VAL)
+			maxlen = fcnt;	/* complete transfer */
+		else
+			maxlen = B_FIFO_SIZE + B_SUB_VAL - le16_to_cpu(*z2r);
+			    /* maximum */
+
+		ptr1 = bdata + (le16_to_cpu(*z2r) - B_SUB_VAL);
+		    /* start of data */
+		memcpy(ptr, ptr1, maxlen);	/* copy data */
+		fcnt -= maxlen;
+
+		if (fcnt) {	/* rest remaining */
+			ptr += maxlen;
+			ptr1 = bdata;	/* start of buffer */
+			memcpy(ptr, ptr1, fcnt);	/* rest */
+		}
+		recv_Bchannel(bch);
+	} else
+		printk(KERN_WARNING "HFCPCI: receive out of memory\n");
+
+	*z2r = cpu_to_le16(new_z2);		/* new position */
+	return 1;
+}
+
+/*
+ * B-channel main receive routine
+ */
+void
+main_rec_hfcpci(struct bchannel *bch)
+{
+	struct hfc_pci	*hc = bch->hw;
+	int		rcnt, real_fifo;
+	int		receive, count = 5;
+	struct bzfifo	*bz;
+	u_char		*bdata;
+	struct zt	*zp;
+
+
+	if ((bch->nr & 2) && (!hc->hw.bswapped)) {
+		bz = &((union fifo_area *)(hc->hw.fifos))->b_chans.rxbz_b2;
+		bdata = ((union fifo_area *)(hc->hw.fifos))->b_chans.rxdat_b2;
+		real_fifo = 1;
+	} else {
+		bz = &((union fifo_area *)(hc->hw.fifos))->b_chans.rxbz_b1;
+		bdata = ((union fifo_area *)(hc->hw.fifos))->b_chans.rxdat_b1;
+		real_fifo = 0;
+	}
+Begin:
+	count--;
+	if (bz->f1 != bz->f2) {
+		if (bch->debug & DEBUG_HW_BCHANNEL)
+			printk(KERN_DEBUG "hfcpci rec ch(%x) f1(%d) f2(%d)\n",
+			    bch->nr, bz->f1, bz->f2);
+		zp = &bz->za[bz->f2];
+
+		rcnt = le16_to_cpu(zp->z1) - le16_to_cpu(zp->z2);
+		if (rcnt < 0)
+			rcnt += B_FIFO_SIZE;
+		rcnt++;
+		if (bch->debug & DEBUG_HW_BCHANNEL)
+			printk(KERN_DEBUG
+			    "hfcpci rec ch(%x) z1(%x) z2(%x) cnt(%d)\n",
+			    bch->nr, le16_to_cpu(zp->z1),
+			    le16_to_cpu(zp->z2), rcnt);
+		hfcpci_empty_bfifo(bch, bz, bdata, rcnt);
+		rcnt = bz->f1 - bz->f2;
+		if (rcnt < 0)
+			rcnt += MAX_B_FRAMES + 1;
+		if (hc->hw.last_bfifo_cnt[real_fifo] > rcnt + 1) {
+			rcnt = 0;
+			hfcpci_clear_fifo_rx(hc, real_fifo);
+		}
+		hc->hw.last_bfifo_cnt[real_fifo] = rcnt;
+		if (rcnt > 1)
+			receive = 1;
+		else
+			receive = 0;
+	} else if (test_bit(FLG_TRANSPARENT, &bch->Flags))
+		receive = hfcpci_empty_fifo_trans(bch, bz, bdata);
+	else
+		receive = 0;
+	if (count && receive)
+		goto Begin;
+
+}
+
+/*
+ * D-channel send routine
+ */
+static void
+hfcpci_fill_dfifo(struct hfc_pci *hc)
+{
+	struct dchannel	*dch = &hc->dch;
+	int		fcnt;
+	int		count, new_z1, maxlen;
+	struct dfifo	*df;
+	u_char		*src, *dst, new_f1;
+
+	if ((dch->debug & DEBUG_HW_DCHANNEL) && !(dch->debug & DEBUG_HW_DFIFO))
+		printk(KERN_DEBUG "%s\n", __func__);
+
+	if (!dch->tx_skb)
+		return;
+	count = dch->tx_skb->len - dch->tx_idx;
+	if (count <= 0)
+		return;
+	df = &((union fifo_area *) (hc->hw.fifos))->d_chan.d_tx;
+
+	if (dch->debug & DEBUG_HW_DFIFO)
+		printk(KERN_DEBUG "%s:f1(%d) f2(%d) z1(f1)(%x)\n", __func__,
+		    df->f1, df->f2,
+		    le16_to_cpu(df->za[df->f1 & D_FREG_MASK].z1));
+	fcnt = df->f1 - df->f2;	/* frame count actually buffered */
+	if (fcnt < 0)
+		fcnt += (MAX_D_FRAMES + 1);	/* if wrap around */
+	if (fcnt > (MAX_D_FRAMES - 1)) {
+		if (dch->debug & DEBUG_HW_DCHANNEL)
+			printk(KERN_DEBUG
+			    "hfcpci_fill_Dfifo more as 14 frames\n");
+#ifdef ERROR_STATISTIC
+		cs->err_tx++;
+#endif
+		return;
+	}
+	/* now determine free bytes in FIFO buffer */
+	maxlen = le16_to_cpu(df->za[df->f2 & D_FREG_MASK].z2) -
+	    le16_to_cpu(df->za[df->f1 & D_FREG_MASK].z1) - 1;
+	if (maxlen <= 0)
+		maxlen += D_FIFO_SIZE;	/* count now contains available bytes */
+
+	if (dch->debug & DEBUG_HW_DCHANNEL)
+		printk(KERN_DEBUG "hfcpci_fill_Dfifo count(%d/%d)\n",
+			count, maxlen);
+	if (count > maxlen) {
+		if (dch->debug & DEBUG_HW_DCHANNEL)
+			printk(KERN_DEBUG "hfcpci_fill_Dfifo no fifo mem\n");
+		return;
+	}
+	new_z1 = (le16_to_cpu(df->za[df->f1 & D_FREG_MASK].z1) + count) &
+	    (D_FIFO_SIZE - 1);
+	new_f1 = ((df->f1 + 1) & D_FREG_MASK) | (D_FREG_MASK + 1);
+	src = dch->tx_skb->data + dch->tx_idx;	/* source pointer */
+	dst = df->data + le16_to_cpu(df->za[df->f1 & D_FREG_MASK].z1);
+	maxlen = D_FIFO_SIZE - le16_to_cpu(df->za[df->f1 & D_FREG_MASK].z1);
+	    /* end fifo */
+	if (maxlen > count)
+		maxlen = count;	/* limit size */
+	memcpy(dst, src, maxlen);	/* first copy */
+
+	count -= maxlen;	/* remaining bytes */
+	if (count) {
+		dst = df->data;	/* start of buffer */
+		src += maxlen;	/* new position */
+		memcpy(dst, src, count);
+	}
+	df->za[new_f1 & D_FREG_MASK].z1 = cpu_to_le16(new_z1);
+	    /* for next buffer */
+	df->za[df->f1 & D_FREG_MASK].z1 = cpu_to_le16(new_z1);
+	    /* new pos actual buffer */
+	df->f1 = new_f1;	/* next frame */
+	dch->tx_idx = dch->tx_skb->len;
+}
+
+/*
+ * B-channel send routine
+ */
+static void
+hfcpci_fill_fifo(struct bchannel *bch)
+{
+	struct hfc_pci 	*hc = bch->hw;
+	int		maxlen, fcnt;
+	int		count, new_z1;
+	struct bzfifo	*bz;
+	u_char		*bdata;
+	u_char		new_f1, *src, *dst;
+	unsigned short	*z1t, *z2t;
+
+	if ((bch->debug & DEBUG_HW_BCHANNEL) && !(bch->debug & DEBUG_HW_BFIFO))
+		printk(KERN_DEBUG "%s\n", __func__);
+	if ((!bch->tx_skb) || bch->tx_skb->len <= 0)
+		return;
+	count = bch->tx_skb->len - bch->tx_idx;
+	if ((bch->nr & 2) && (!hc->hw.bswapped)) {
+		bz = &((union fifo_area *)(hc->hw.fifos))->b_chans.txbz_b2;
+		bdata = ((union fifo_area *)(hc->hw.fifos))->b_chans.txdat_b2;
+	} else {
+		bz = &((union fifo_area *)(hc->hw.fifos))->b_chans.txbz_b1;
+		bdata = ((union fifo_area *)(hc->hw.fifos))->b_chans.txdat_b1;
+	}
+
+	if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
+		z1t = &bz->za[MAX_B_FRAMES].z1;
+		z2t = z1t + 1;
+		if (bch->debug & DEBUG_HW_BCHANNEL)
+			printk(KERN_DEBUG "hfcpci_fill_fifo_trans ch(%x) "
+			    "cnt(%d) z1(%x) z2(%x)\n", bch->nr, count,
+			    le16_to_cpu(*z1t), le16_to_cpu(*z2t));
+		fcnt = le16_to_cpu(*z2t) - le16_to_cpu(*z1t);
+		if (fcnt <= 0)
+			fcnt += B_FIFO_SIZE;
+			    /* fcnt contains available bytes in fifo */
+		fcnt = B_FIFO_SIZE - fcnt;
+		    /* remaining bytes to send (bytes in fifo) */
+next_t_frame:
+		count = bch->tx_skb->len - bch->tx_idx;
+		/* maximum fill shall be HFCPCI_BTRANS_MAX */
+		if (count > HFCPCI_BTRANS_MAX - fcnt)
+			count = HFCPCI_BTRANS_MAX - fcnt;
+		if (count <= 0)
+			return;
+		/* data is suitable for fifo */
+		new_z1 = le16_to_cpu(*z1t) + count;
+		    /* new buffer Position */
+		if (new_z1 >= (B_FIFO_SIZE + B_SUB_VAL))
+			new_z1 -= B_FIFO_SIZE;	/* buffer wrap */
+		src = bch->tx_skb->data + bch->tx_idx;
+		    /* source pointer */
+		dst = bdata + (le16_to_cpu(*z1t) - B_SUB_VAL);
+		maxlen = (B_FIFO_SIZE + B_SUB_VAL) - le16_to_cpu(*z1t);
+		    /* end of fifo */
+		if (bch->debug & DEBUG_HW_BFIFO)
+			printk(KERN_DEBUG "hfcpci_FFt fcnt(%d) "
+			    "maxl(%d) nz1(%x) dst(%p)\n",
+			    fcnt, maxlen, new_z1, dst);
+		fcnt += count;
+		bch->tx_idx += count;
+		if (maxlen > count)
+			maxlen = count;		/* limit size */
+		memcpy(dst, src, maxlen);	/* first copy */
+		count -= maxlen;	/* remaining bytes */
+		if (count) {
+			dst = bdata;	/* start of buffer */
+			src += maxlen;	/* new position */
+			memcpy(dst, src, count);
+		}
+		*z1t = cpu_to_le16(new_z1);	/* now send data */
+		if (bch->tx_idx < bch->tx_skb->len)
+			return;
+		/* send confirm, on trans, free on hdlc. */
+		if (test_bit(FLG_TRANSPARENT, &bch->Flags))
+			confirm_Bsend(bch);
+		dev_kfree_skb(bch->tx_skb);
+		if (get_next_bframe(bch))
+			goto next_t_frame;
+		return;
+	}
+	if (bch->debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG
+		    "%s: ch(%x) f1(%d) f2(%d) z1(f1)(%x)\n",
+		    __func__, bch->nr, bz->f1, bz->f2,
+		    bz->za[bz->f1].z1);
+	fcnt = bz->f1 - bz->f2;	/* frame count actually buffered */
+	if (fcnt < 0)
+		fcnt += (MAX_B_FRAMES + 1);	/* if wrap around */
+	if (fcnt > (MAX_B_FRAMES - 1)) {
+		if (bch->debug & DEBUG_HW_BCHANNEL)
+			printk(KERN_DEBUG
+			    "hfcpci_fill_Bfifo more as 14 frames\n");
+		return;
+	}
+	/* now determine free bytes in FIFO buffer */
+	maxlen = le16_to_cpu(bz->za[bz->f2].z2) -
+	    le16_to_cpu(bz->za[bz->f1].z1) - 1;
+	if (maxlen <= 0)
+		maxlen += B_FIFO_SIZE;	/* count now contains available bytes */
+
+	if (bch->debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG "hfcpci_fill_fifo ch(%x) count(%d/%d)\n",
+			bch->nr, count, maxlen);
+
+	if (maxlen < count) {
+		if (bch->debug & DEBUG_HW_BCHANNEL)
+			printk(KERN_DEBUG "hfcpci_fill_fifo no fifo mem\n");
+		return;
+	}
+	new_z1 = le16_to_cpu(bz->za[bz->f1].z1) + count;
+	    /* new buffer Position */
+	if (new_z1 >= (B_FIFO_SIZE + B_SUB_VAL))
+		new_z1 -= B_FIFO_SIZE;	/* buffer wrap */
+
+	new_f1 = ((bz->f1 + 1) & MAX_B_FRAMES);
+	src = bch->tx_skb->data + bch->tx_idx;	/* source pointer */
+	dst = bdata + (le16_to_cpu(bz->za[bz->f1].z1) - B_SUB_VAL);
+	maxlen = (B_FIFO_SIZE + B_SUB_VAL) - le16_to_cpu(bz->za[bz->f1].z1);
+	    /* end fifo */
+	if (maxlen > count)
+		maxlen = count;	/* limit size */
+	memcpy(dst, src, maxlen);	/* first copy */
+
+	count -= maxlen;	/* remaining bytes */
+	if (count) {
+		dst = bdata;	/* start of buffer */
+		src += maxlen;	/* new position */
+		memcpy(dst, src, count);
+	}
+	bz->za[new_f1].z1 = cpu_to_le16(new_z1);	/* for next buffer */
+	bz->f1 = new_f1;	/* next frame */
+	dev_kfree_skb(bch->tx_skb);
+	get_next_bframe(bch);
+}
+
+
+
+/*
+ * handle L1 state changes TE
+ */
+
+static void
+ph_state_te(struct dchannel *dch)
+{
+	if (dch->debug)
+		printk(KERN_DEBUG "%s: TE newstate %x\n",
+			__func__, dch->state);
+	switch (dch->state) {
+	case 0:
+		l1_event(dch->l1, HW_RESET_IND);
+		break;
+	case 3:
+		l1_event(dch->l1, HW_DEACT_IND);
+		break;
+	case 5:
+	case 8:
+		l1_event(dch->l1, ANYSIGNAL);
+		break;
+	case 6:
+		l1_event(dch->l1, INFO2);
+		break;
+	case 7:
+		l1_event(dch->l1, INFO4_P8);
+		break;
+	}
+}
+
+/*
+ * handle L1 state changes NT
+ */
+
+static void
+handle_nt_timer3(struct dchannel *dch) {
+	struct hfc_pci	*hc = dch->hw;
+
+	test_and_clear_bit(FLG_HFC_TIMER_T3, &dch->Flags);
+	hc->hw.int_m1 &= ~HFCPCI_INTS_TIMER;
+	Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+	hc->hw.nt_timer = 0;
+	test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+	if (test_bit(HFC_CFG_MASTER, &hc->cfg))
+		hc->hw.mst_m |= HFCPCI_MASTER;
+	Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+	_queue_data(&dch->dev.D, PH_ACTIVATE_IND,
+	    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+}
+
+static void
+ph_state_nt(struct dchannel *dch)
+{
+	struct hfc_pci	*hc = dch->hw;
+
+	if (dch->debug)
+		printk(KERN_DEBUG "%s: NT newstate %x\n",
+			__func__, dch->state);
+	switch (dch->state) {
+	case 2:
+		if (hc->hw.nt_timer < 0) {
+			hc->hw.nt_timer = 0;
+			test_and_clear_bit(FLG_HFC_TIMER_T3, &dch->Flags);
+			test_and_clear_bit(FLG_HFC_TIMER_T1, &dch->Flags);
+			hc->hw.int_m1 &= ~HFCPCI_INTS_TIMER;
+			Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+			/* Clear already pending ints */
+			if (Read_hfc(hc, HFCPCI_INT_S1));
+			Write_hfc(hc, HFCPCI_STATES, 4 | HFCPCI_LOAD_STATE);
+			udelay(10);
+			Write_hfc(hc, HFCPCI_STATES, 4);
+			dch->state = 4;
+		} else if (hc->hw.nt_timer == 0) {
+			hc->hw.int_m1 |= HFCPCI_INTS_TIMER;
+			Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+			hc->hw.nt_timer = NT_T1_COUNT;
+			hc->hw.ctmt &= ~HFCPCI_AUTO_TIMER;
+			hc->hw.ctmt |= HFCPCI_TIM3_125;
+			Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt |
+				HFCPCI_CLTIMER);
+			test_and_clear_bit(FLG_HFC_TIMER_T3, &dch->Flags);
+			test_and_set_bit(FLG_HFC_TIMER_T1, &dch->Flags);
+			/* allow G2 -> G3 transition */
+			Write_hfc(hc, HFCPCI_STATES, 2 | HFCPCI_NT_G2_G3);
+		} else {
+			Write_hfc(hc, HFCPCI_STATES, 2 | HFCPCI_NT_G2_G3);
+		}
+		break;
+	case 1:
+		hc->hw.nt_timer = 0;
+		test_and_clear_bit(FLG_HFC_TIMER_T3, &dch->Flags);
+		test_and_clear_bit(FLG_HFC_TIMER_T1, &dch->Flags);
+		hc->hw.int_m1 &= ~HFCPCI_INTS_TIMER;
+		Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+		test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+		hc->hw.mst_m &= ~HFCPCI_MASTER;
+		Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+		test_and_clear_bit(FLG_L2_ACTIVATED, &dch->Flags);
+		_queue_data(&dch->dev.D, PH_DEACTIVATE_IND,
+		    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+		break;
+	case 4:
+		hc->hw.nt_timer = 0;
+		test_and_clear_bit(FLG_HFC_TIMER_T3, &dch->Flags);
+		test_and_clear_bit(FLG_HFC_TIMER_T1, &dch->Flags);
+		hc->hw.int_m1 &= ~HFCPCI_INTS_TIMER;
+		Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+		break;
+	case 3:
+		if (!test_and_set_bit(FLG_HFC_TIMER_T3, &dch->Flags)) {
+			if (!test_and_clear_bit(FLG_L2_ACTIVATED,
+			    &dch->Flags)) {
+				handle_nt_timer3(dch);
+				break;
+			}
+			test_and_clear_bit(FLG_HFC_TIMER_T1, &dch->Flags);
+			hc->hw.int_m1 |= HFCPCI_INTS_TIMER;
+			Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+			hc->hw.nt_timer = NT_T3_COUNT;
+			hc->hw.ctmt &= ~HFCPCI_AUTO_TIMER;
+			hc->hw.ctmt |= HFCPCI_TIM3_125;
+			Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt |
+				HFCPCI_CLTIMER);
+		}
+		break;
+	}
+}
+
+static void
+ph_state(struct dchannel *dch)
+{
+	struct hfc_pci	*hc = dch->hw;
+
+	if (hc->hw.protocol == ISDN_P_NT_S0) {
+		if (test_bit(FLG_HFC_TIMER_T3, &dch->Flags) &&
+		    hc->hw.nt_timer < 0)
+			handle_nt_timer3(dch);
+		else
+			ph_state_nt(dch);
+	} else
+		ph_state_te(dch);
+}
+
+/*
+ * Layer 1 callback function
+ */
+static int
+hfc_l1callback(struct dchannel *dch, u_int cmd)
+{
+	struct hfc_pci		*hc = dch->hw;
+
+	switch (cmd) {
+	case INFO3_P8:
+	case INFO3_P10:
+		if (test_bit(HFC_CFG_MASTER, &hc->cfg))
+			hc->hw.mst_m |= HFCPCI_MASTER;
+		Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+		break;
+	case HW_RESET_REQ:
+		Write_hfc(hc, HFCPCI_STATES, HFCPCI_LOAD_STATE | 3);
+		/* HFC ST 3 */
+		udelay(6);
+		Write_hfc(hc, HFCPCI_STATES, 3);	/* HFC ST 2 */
+		if (test_bit(HFC_CFG_MASTER, &hc->cfg))
+			hc->hw.mst_m |= HFCPCI_MASTER;
+		Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+		Write_hfc(hc, HFCPCI_STATES, HFCPCI_ACTIVATE |
+		   HFCPCI_DO_ACTION);
+		l1_event(dch->l1, HW_POWERUP_IND);
+		break;
+	case HW_DEACT_REQ:
+		hc->hw.mst_m &= ~HFCPCI_MASTER;
+		Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+		skb_queue_purge(&dch->squeue);
+		if (dch->tx_skb) {
+			dev_kfree_skb(dch->tx_skb);
+			dch->tx_skb = NULL;
+		}
+		dch->tx_idx = 0;
+		if (dch->rx_skb) {
+			dev_kfree_skb(dch->rx_skb);
+			dch->rx_skb = NULL;
+		}
+		test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
+		if (test_and_clear_bit(FLG_BUSY_TIMER, &dch->Flags))
+			del_timer(&dch->timer);
+		break;
+	case HW_POWERUP_REQ:
+		Write_hfc(hc, HFCPCI_STATES, HFCPCI_DO_ACTION);
+		break;
+	case PH_ACTIVATE_IND:
+		test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, cmd, MISDN_ID_ANY, 0, NULL,
+			GFP_ATOMIC);
+		break;
+	case PH_DEACTIVATE_IND:
+		test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, cmd, MISDN_ID_ANY, 0, NULL,
+			GFP_ATOMIC);
+		break;
+	default:
+		if (dch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "%s: unknown command %x\n",
+			    __func__, cmd);
+		return -1;
+	}
+	return 0;
+}
+
+/*
+ * Interrupt handler
+ */
+static inline void
+tx_birq(struct bchannel *bch)
+{
+	if (bch->tx_skb && bch->tx_idx < bch->tx_skb->len)
+		hfcpci_fill_fifo(bch);
+	else {
+		if (bch->tx_skb)
+			dev_kfree_skb(bch->tx_skb);
+		if (get_next_bframe(bch))
+			hfcpci_fill_fifo(bch);
+	}
+}
+
+static inline void
+tx_dirq(struct dchannel *dch)
+{
+	if (dch->tx_skb && dch->tx_idx < dch->tx_skb->len)
+		hfcpci_fill_dfifo(dch->hw);
+	else {
+		if (dch->tx_skb)
+			dev_kfree_skb(dch->tx_skb);
+		if (get_next_dframe(dch))
+			hfcpci_fill_dfifo(dch->hw);
+	}
+}
+
+static irqreturn_t
+hfcpci_int(int intno, void *dev_id)
+{
+	struct hfc_pci	*hc = dev_id;
+	u_char		exval;
+	struct bchannel	*bch;
+	u_char		val, stat;
+
+	spin_lock(&hc->lock);
+	if (!(hc->hw.int_m2 & 0x08)) {
+		spin_unlock(&hc->lock);
+		return IRQ_NONE; /* not initialised */
+	}
+	stat = Read_hfc(hc, HFCPCI_STATUS);
+	if (HFCPCI_ANYINT & stat) {
+		val = Read_hfc(hc, HFCPCI_INT_S1);
+		if (hc->dch.debug & DEBUG_HW_DCHANNEL)
+			printk(KERN_DEBUG
+			    "HFC-PCI: stat(%02x) s1(%02x)\n", stat, val);
+	} else {
+		/* shared */
+		spin_unlock(&hc->lock);
+		return IRQ_NONE;
+	}
+	hc->irqcnt++;
+
+	if (hc->dch.debug & DEBUG_HW_DCHANNEL)
+		printk(KERN_DEBUG "HFC-PCI irq %x\n", val);
+	val &= hc->hw.int_m1;
+	if (val & 0x40) {	/* state machine irq */
+		exval = Read_hfc(hc, HFCPCI_STATES) & 0xf;
+		if (hc->dch.debug & DEBUG_HW_DCHANNEL)
+			printk(KERN_DEBUG "ph_state chg %d->%d\n",
+				hc->dch.state, exval);
+		hc->dch.state = exval;
+		schedule_event(&hc->dch, FLG_PHCHANGE);
+		val &= ~0x40;
+	}
+	if (val & 0x80) {	/* timer irq */
+		if (hc->hw.protocol == ISDN_P_NT_S0) {
+			if ((--hc->hw.nt_timer) < 0)
+				schedule_event(&hc->dch, FLG_PHCHANGE);
+		}
+		val &= ~0x80;
+		Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt | HFCPCI_CLTIMER);
+	}
+	if (val & 0x08) {
+		bch = Sel_BCS(hc, hc->hw.bswapped ? 2 : 1);
+		if (bch)
+			main_rec_hfcpci(bch);
+		else if (hc->dch.debug)
+			printk(KERN_DEBUG "hfcpci spurious 0x08 IRQ\n");
+	}
+	if (val & 0x10) {
+		bch = Sel_BCS(hc, 2);
+		if (bch)
+			main_rec_hfcpci(bch);
+		else if (hc->dch.debug)
+			printk(KERN_DEBUG "hfcpci spurious 0x10 IRQ\n");
+	}
+	if (val & 0x01) {
+		bch = Sel_BCS(hc, hc->hw.bswapped ? 2 : 1);
+		if (bch)
+			tx_birq(bch);
+		else if (hc->dch.debug)
+			printk(KERN_DEBUG "hfcpci spurious 0x01 IRQ\n");
+	}
+	if (val & 0x02) {
+		bch = Sel_BCS(hc, 2);
+		if (bch)
+			tx_birq(bch);
+		else if (hc->dch.debug)
+			printk(KERN_DEBUG "hfcpci spurious 0x02 IRQ\n");
+	}
+	if (val & 0x20)
+		receive_dmsg(hc);
+	if (val & 0x04) {	/* dframe transmitted */
+		if (test_and_clear_bit(FLG_BUSY_TIMER, &hc->dch.Flags))
+			del_timer(&hc->dch.timer);
+		tx_dirq(&hc->dch);
+	}
+	spin_unlock(&hc->lock);
+	return IRQ_HANDLED;
+}
+
+/*
+ * timer callback for D-chan busy resolution. Currently no function
+ */
+static void
+hfcpci_dbusy_timer(struct hfc_pci *hc)
+{
+}
+
+/*
+ * activate/deactivate hardware for selected channels and mode
+ */
+static int
+mode_hfcpci(struct bchannel *bch, int bc, int protocol)
+{
+	struct hfc_pci	*hc = bch->hw;
+	int		fifo2;
+	u_char		rx_slot = 0, tx_slot = 0, pcm_mode;
+
+	if (bch->debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG
+		    "HFCPCI bchannel protocol %x-->%x ch %x-->%x\n",
+		    bch->state, protocol, bch->nr, bc);
+
+	fifo2 = bc;
+	pcm_mode = (bc>>24) & 0xff;
+	if (pcm_mode) { /* PCM SLOT USE */
+		if (!test_bit(HFC_CFG_PCM, &hc->cfg))
+			printk(KERN_WARNING
+			    "%s: pcm channel id without HFC_CFG_PCM\n",
+			    __func__);
+		rx_slot = (bc>>8) & 0xff;
+		tx_slot = (bc>>16) & 0xff;
+		bc = bc & 0xff;
+	} else if (test_bit(HFC_CFG_PCM, &hc->cfg) &&
+	    (protocol > ISDN_P_NONE))
+		printk(KERN_WARNING "%s: no pcm channel id but HFC_CFG_PCM\n",
+		    __func__);
+	if (hc->chanlimit > 1) {
+		hc->hw.bswapped = 0;	/* B1 and B2 normal mode */
+		hc->hw.sctrl_e &= ~0x80;
+	} else {
+		if (bc & 2) {
+			if (protocol != ISDN_P_NONE) {
+				hc->hw.bswapped = 1; /* B1 and B2 exchanged */
+				hc->hw.sctrl_e |= 0x80;
+			} else {
+				hc->hw.bswapped = 0; /* B1 and B2 normal mode */
+				hc->hw.sctrl_e &= ~0x80;
+			}
+			fifo2 = 1;
+		} else {
+			hc->hw.bswapped = 0;	/* B1 and B2 normal mode */
+			hc->hw.sctrl_e &= ~0x80;
+		}
+	}
+	switch (protocol) {
+	case (-1): /* used for init */
+		bch->state = -1;
+		bch->nr = bc;
+	case (ISDN_P_NONE):
+		if (bch->state == ISDN_P_NONE)
+			return 0;
+		if (bc & 2) {
+			hc->hw.sctrl &= ~SCTRL_B2_ENA;
+			hc->hw.sctrl_r &= ~SCTRL_B2_ENA;
+		} else {
+			hc->hw.sctrl &= ~SCTRL_B1_ENA;
+			hc->hw.sctrl_r &= ~SCTRL_B1_ENA;
+		}
+		if (fifo2 & 2) {
+			hc->hw.fifo_en &= ~HFCPCI_FIFOEN_B2;
+			hc->hw.int_m1 &= ~(HFCPCI_INTS_B2TRANS +
+				HFCPCI_INTS_B2REC);
+		} else {
+			hc->hw.fifo_en &= ~HFCPCI_FIFOEN_B1;
+			hc->hw.int_m1 &= ~(HFCPCI_INTS_B1TRANS +
+				HFCPCI_INTS_B1REC);
+		}
+#ifdef REVERSE_BITORDER
+		if (bch->nr & 2)
+			hc->hw.cirm &= 0x7f;
+		else
+			hc->hw.cirm &= 0xbf;
+#endif
+		bch->state = ISDN_P_NONE;
+		bch->nr = bc;
+		test_and_clear_bit(FLG_HDLC, &bch->Flags);
+		test_and_clear_bit(FLG_TRANSPARENT, &bch->Flags);
+		break;
+	case (ISDN_P_B_RAW):
+		bch->state = protocol;
+		bch->nr = bc;
+		hfcpci_clear_fifo_rx(hc, (fifo2 & 2)?1:0);
+		hfcpci_clear_fifo_tx(hc, (fifo2 & 2)?1:0);
+		if (bc & 2) {
+			hc->hw.sctrl |= SCTRL_B2_ENA;
+			hc->hw.sctrl_r |= SCTRL_B2_ENA;
+#ifdef REVERSE_BITORDER
+			hc->hw.cirm |= 0x80;
+#endif
+		} else {
+			hc->hw.sctrl |= SCTRL_B1_ENA;
+			hc->hw.sctrl_r |= SCTRL_B1_ENA;
+#ifdef REVERSE_BITORDER
+			hc->hw.cirm |= 0x40;
+#endif
+		}
+		if (fifo2 & 2) {
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B2;
+			hc->hw.int_m1 |= (HFCPCI_INTS_B2TRANS +
+			    HFCPCI_INTS_B2REC);
+			hc->hw.ctmt |= 2;
+			hc->hw.conn &= ~0x18;
+		} else {
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B1;
+			hc->hw.int_m1 |= (HFCPCI_INTS_B1TRANS +
+			    HFCPCI_INTS_B1REC);
+			hc->hw.ctmt |= 1;
+			hc->hw.conn &= ~0x03;
+		}
+		test_and_set_bit(FLG_TRANSPARENT, &bch->Flags);
+		break;
+	case (ISDN_P_B_HDLC):
+		bch->state = protocol;
+		bch->nr = bc;
+		hfcpci_clear_fifo_rx(hc, (fifo2 & 2)?1:0);
+		hfcpci_clear_fifo_tx(hc, (fifo2 & 2)?1:0);
+		if (bc & 2) {
+			hc->hw.sctrl |= SCTRL_B2_ENA;
+			hc->hw.sctrl_r |= SCTRL_B2_ENA;
+		} else {
+			hc->hw.sctrl |= SCTRL_B1_ENA;
+			hc->hw.sctrl_r |= SCTRL_B1_ENA;
+		}
+		if (fifo2 & 2) {
+			hc->hw.last_bfifo_cnt[1] = 0;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B2;
+			hc->hw.int_m1 |= (HFCPCI_INTS_B2TRANS +
+			    HFCPCI_INTS_B2REC);
+			hc->hw.ctmt &= ~2;
+			hc->hw.conn &= ~0x18;
+		} else {
+			hc->hw.last_bfifo_cnt[0] = 0;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B1;
+			hc->hw.int_m1 |= (HFCPCI_INTS_B1TRANS +
+			    HFCPCI_INTS_B1REC);
+			hc->hw.ctmt &= ~1;
+			hc->hw.conn &= ~0x03;
+		}
+		test_and_set_bit(FLG_HDLC, &bch->Flags);
+		break;
+	default:
+		printk(KERN_DEBUG "prot not known %x\n", protocol);
+		return -ENOPROTOOPT;
+	}
+	if (test_bit(HFC_CFG_PCM, &hc->cfg)) {
+		if ((protocol == ISDN_P_NONE) ||
+			(protocol == -1)) {	/* init case */
+			rx_slot = 0;
+			tx_slot = 0;
+		} else {
+			if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg)) {
+				rx_slot |= 0xC0;
+				tx_slot |= 0xC0;
+			} else {
+				rx_slot |= 0x80;
+				tx_slot |= 0x80;
+			}
+		}
+		if (bc & 2) {
+			hc->hw.conn &= 0xc7;
+			hc->hw.conn |= 0x08;
+			printk(KERN_DEBUG "%s: Write_hfc: B2_SSL 0x%x\n",
+				__func__, tx_slot);
+			printk(KERN_DEBUG "%s: Write_hfc: B2_RSL 0x%x\n",
+				__func__, rx_slot);
+			Write_hfc(hc, HFCPCI_B2_SSL, tx_slot);
+			Write_hfc(hc, HFCPCI_B2_RSL, rx_slot);
+		} else {
+			hc->hw.conn &= 0xf8;
+			hc->hw.conn |= 0x01;
+			printk(KERN_DEBUG "%s: Write_hfc: B1_SSL 0x%x\n",
+				__func__, tx_slot);
+			printk(KERN_DEBUG "%s: Write_hfc: B1_RSL 0x%x\n",
+				__func__, rx_slot);
+			Write_hfc(hc, HFCPCI_B1_SSL, tx_slot);
+			Write_hfc(hc, HFCPCI_B1_RSL, rx_slot);
+		}
+	}
+	Write_hfc(hc, HFCPCI_SCTRL_E, hc->hw.sctrl_e);
+	Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	Write_hfc(hc, HFCPCI_SCTRL, hc->hw.sctrl);
+	Write_hfc(hc, HFCPCI_SCTRL_R, hc->hw.sctrl_r);
+	Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt);
+	Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+#ifdef REVERSE_BITORDER
+	Write_hfc(hc, HFCPCI_CIRM, hc->hw.cirm);
+#endif
+	return 0;
+}
+
+static int
+set_hfcpci_rxtest(struct bchannel *bch, int protocol, int chan)
+{
+	struct hfc_pci	*hc = bch->hw;
+
+	if (bch->debug & DEBUG_HW_BCHANNEL)
+		printk(KERN_DEBUG
+		    "HFCPCI bchannel test rx protocol %x-->%x ch %x-->%x\n",
+		    bch->state, protocol, bch->nr, chan);
+	if (bch->nr != chan) {
+		printk(KERN_DEBUG
+		    "HFCPCI rxtest wrong channel parameter %x/%x\n",
+		    bch->nr, chan);
+		return -EINVAL;
+	}
+	switch (protocol) {
+	case (ISDN_P_B_RAW):
+		bch->state = protocol;
+		hfcpci_clear_fifo_rx(hc, (chan & 2)?1:0);
+		if (chan & 2) {
+			hc->hw.sctrl_r |= SCTRL_B2_ENA;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B2RX;
+			hc->hw.int_m1 |= HFCPCI_INTS_B2REC;
+			hc->hw.ctmt |= 2;
+			hc->hw.conn &= ~0x18;
+#ifdef REVERSE_BITORDER
+			hc->hw.cirm |= 0x80;
+#endif
+		} else {
+			hc->hw.sctrl_r |= SCTRL_B1_ENA;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B1RX;
+			hc->hw.int_m1 |= HFCPCI_INTS_B1REC;
+			hc->hw.ctmt |= 1;
+			hc->hw.conn &= ~0x03;
+#ifdef REVERSE_BITORDER
+			hc->hw.cirm |= 0x40;
+#endif
+		}
+		break;
+	case (ISDN_P_B_HDLC):
+		bch->state = protocol;
+		hfcpci_clear_fifo_rx(hc, (chan & 2)?1:0);
+		if (chan & 2) {
+			hc->hw.sctrl_r |= SCTRL_B2_ENA;
+			hc->hw.last_bfifo_cnt[1] = 0;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B2RX;
+			hc->hw.int_m1 |= HFCPCI_INTS_B2REC;
+			hc->hw.ctmt &= ~2;
+			hc->hw.conn &= ~0x18;
+		} else {
+			hc->hw.sctrl_r |= SCTRL_B1_ENA;
+			hc->hw.last_bfifo_cnt[0] = 0;
+			hc->hw.fifo_en |= HFCPCI_FIFOEN_B1RX;
+			hc->hw.int_m1 |= HFCPCI_INTS_B1REC;
+			hc->hw.ctmt &= ~1;
+			hc->hw.conn &= ~0x03;
+		}
+		break;
+	default:
+		printk(KERN_DEBUG "prot not known %x\n", protocol);
+		return -ENOPROTOOPT;
+	}
+	Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+	Write_hfc(hc, HFCPCI_FIFO_EN, hc->hw.fifo_en);
+	Write_hfc(hc, HFCPCI_SCTRL_R, hc->hw.sctrl_r);
+	Write_hfc(hc, HFCPCI_CTMT, hc->hw.ctmt);
+	Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+#ifdef REVERSE_BITORDER
+	Write_hfc(hc, HFCPCI_CIRM, hc->hw.cirm);
+#endif
+	return 0;
+}
+
+static void
+deactivate_bchannel(struct bchannel *bch)
+{
+	struct hfc_pci	*hc = bch->hw;
+	u_long		flags;
+
+	spin_lock_irqsave(&hc->lock, flags);
+	if (test_and_clear_bit(FLG_TX_NEXT, &bch->Flags)) {
+		dev_kfree_skb(bch->next_skb);
+		bch->next_skb = NULL;
+	}
+	if (bch->tx_skb) {
+		dev_kfree_skb(bch->tx_skb);
+		bch->tx_skb = NULL;
+	}
+	bch->tx_idx = 0;
+	if (bch->rx_skb) {
+		dev_kfree_skb(bch->rx_skb);
+		bch->rx_skb = NULL;
+	}
+	mode_hfcpci(bch, bch->nr, ISDN_P_NONE);
+	test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
+	test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
+	spin_unlock_irqrestore(&hc->lock, flags);
+}
+
+/*
+ * Layer 1 B-channel hardware access
+ */
+static int
+channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
+{
+	int			ret = 0;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = 0;
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n", __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+static int
+hfc_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct bchannel	*bch = container_of(ch, struct bchannel, ch);
+	struct hfc_pci	*hc = bch->hw;
+	int		ret = -EINVAL;
+	u_long		flags;
+
+	if (bch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n", __func__, cmd, arg);
+	switch (cmd) {
+	case HW_TESTRX_RAW:
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = set_hfcpci_rxtest(bch, ISDN_P_B_RAW, (int)(long)arg);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case HW_TESTRX_HDLC:
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = set_hfcpci_rxtest(bch, ISDN_P_B_HDLC, (int)(long)arg);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case HW_TESTRX_OFF:
+		spin_lock_irqsave(&hc->lock, flags);
+		mode_hfcpci(bch, bch->nr, ISDN_P_NONE);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		ret = 0;
+		break;
+	case CLOSE_CHANNEL:
+		test_and_clear_bit(FLG_OPEN, &bch->Flags);
+		if (test_bit(FLG_ACTIVE, &bch->Flags))
+			deactivate_bchannel(bch);
+		ch->protocol = ISDN_P_NONE;
+		ch->peer = NULL;
+		module_put(THIS_MODULE);
+		ret = 0;
+		break;
+	case CONTROL_CHANNEL:
+		ret = channel_bctrl(bch, arg);
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown prim(%x)\n",
+			__func__, cmd);
+	}
+	return ret;
+}
+
+/*
+ * Layer2 -> Layer 1 Dchannel data
+ */
+static int
+hfcpci_l2l1D(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct hfc_pci		*hc = dch->hw;
+	int			ret = -EINVAL;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	unsigned int		id;
+	u_long			flags;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = dchannel_senddata(dch, skb);
+		if (ret > 0) { /* direct TX */
+			id = hh->id; /* skb can be freed */
+			hfcpci_fill_dfifo(dch->hw);
+			ret = 0;
+			spin_unlock_irqrestore(&hc->lock, flags);
+			queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
+		} else
+			spin_unlock_irqrestore(&hc->lock, flags);
+		return ret;
+	case PH_ACTIVATE_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		if (hc->hw.protocol == ISDN_P_NT_S0) {
+			ret = 0;
+			if (test_bit(HFC_CFG_MASTER, &hc->cfg))
+				hc->hw.mst_m |= HFCPCI_MASTER;
+			Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+			if (test_bit(FLG_ACTIVE, &dch->Flags)) {
+				spin_unlock_irqrestore(&hc->lock, flags);
+				_queue_data(&dch->dev.D, PH_ACTIVATE_IND,
+				    MISDN_ID_ANY, 0, NULL, GFP_ATOMIC);
+				break;
+			}
+			test_and_set_bit(FLG_L2_ACTIVATED, &dch->Flags);
+			Write_hfc(hc, HFCPCI_STATES, HFCPCI_ACTIVATE |
+			    HFCPCI_DO_ACTION | 1);
+		} else
+			ret = l1_event(dch->l1, hh->prim);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	case PH_DEACTIVATE_REQ:
+		test_and_clear_bit(FLG_L2_ACTIVATED, &dch->Flags);
+		spin_lock_irqsave(&hc->lock, flags);
+		if (hc->hw.protocol == ISDN_P_NT_S0) {
+			/* prepare deactivation */
+			Write_hfc(hc, HFCPCI_STATES, 0x40);
+			skb_queue_purge(&dch->squeue);
+			if (dch->tx_skb) {
+				dev_kfree_skb(dch->tx_skb);
+				dch->tx_skb = NULL;
+			}
+			dch->tx_idx = 0;
+			if (dch->rx_skb) {
+				dev_kfree_skb(dch->rx_skb);
+				dch->rx_skb = NULL;
+			}
+			test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
+			if (test_and_clear_bit(FLG_BUSY_TIMER, &dch->Flags))
+				del_timer(&dch->timer);
+#ifdef FIXME
+			if (test_and_clear_bit(FLG_L1_BUSY, &dch->Flags))
+				dchannel_sched_event(&hc->dch, D_CLEARBUSY);
+#endif
+			hc->hw.mst_m &= ~HFCPCI_MASTER;
+			Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+			ret = 0;
+		} else {
+			ret = l1_event(dch->l1, hh->prim);
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
+		break;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+/*
+ * Layer2 -> Layer 1 Bchannel data
+ */
+static int
+hfcpci_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct bchannel		*bch = container_of(ch, struct bchannel, ch);
+	struct hfc_pci		*hc = bch->hw;
+	int			ret = -EINVAL;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	unsigned int		id;
+	u_long			flags;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		ret = bchannel_senddata(bch, skb);
+		if (ret > 0) { /* direct TX */
+			id = hh->id; /* skb can be freed */
+			hfcpci_fill_fifo(bch);
+			ret = 0;
+			spin_unlock_irqrestore(&hc->lock, flags);
+			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
+				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
+		} else
+			spin_unlock_irqrestore(&hc->lock, flags);
+		return ret;
+	case PH_ACTIVATE_REQ:
+		spin_lock_irqsave(&hc->lock, flags);
+		if (!test_and_set_bit(FLG_ACTIVE, &bch->Flags))
+			ret = mode_hfcpci(bch, bch->nr, ch->protocol);
+		else
+			ret = 0;
+		spin_unlock_irqrestore(&hc->lock, flags);
+		if (!ret)
+			_queue_data(ch, PH_ACTIVATE_IND, MISDN_ID_ANY, 0,
+				NULL, GFP_KERNEL);
+		break;
+	case PH_DEACTIVATE_REQ:
+		deactivate_bchannel(bch);
+		_queue_data(ch, PH_DEACTIVATE_IND, MISDN_ID_ANY, 0,
+			NULL, GFP_KERNEL);
+		ret = 0;
+		break;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+/*
+ * called for card init message
+ */
+
+void
+inithfcpci(struct hfc_pci *hc)
+{
+	printk(KERN_DEBUG "inithfcpci: entered\n");
+	hc->dch.timer.function = (void *) hfcpci_dbusy_timer;
+	hc->dch.timer.data = (long) &hc->dch;
+	init_timer(&hc->dch.timer);
+	hc->chanlimit = 2;
+	mode_hfcpci(&hc->bch[0], 1, -1);
+	mode_hfcpci(&hc->bch[1], 2, -1);
+}
+
+
+static int
+init_card(struct hfc_pci *hc)
+{
+	int	cnt = 3;
+	u_long	flags;
+
+	printk(KERN_DEBUG "init_card: entered\n");
+
+
+	spin_lock_irqsave(&hc->lock, flags);
+	disable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	if (request_irq(hc->irq, hfcpci_int, IRQF_SHARED, "HFC PCI", hc)) {
+		printk(KERN_WARNING
+		    "mISDN: couldn't get interrupt %d\n", hc->irq);
+		return -EIO;
+	}
+	spin_lock_irqsave(&hc->lock, flags);
+	reset_hfcpci(hc);
+	while (cnt) {
+		inithfcpci(hc);
+		/*
+		 * Finally enable IRQ output
+		 * this is only allowed, if an IRQ routine is allready
+		 * established for this HFC, so don't do that earlier
+		 */
+		enable_hwirq(hc);
+		spin_unlock_irqrestore(&hc->lock, flags);
+		/* Timeout 80ms */
+		current->state = TASK_UNINTERRUPTIBLE;
+		schedule_timeout((80*HZ)/1000);
+		printk(KERN_INFO "HFC PCI: IRQ %d count %d\n",
+			hc->irq, hc->irqcnt);
+		/* now switch timer interrupt off */
+		spin_lock_irqsave(&hc->lock, flags);
+		hc->hw.int_m1 &= ~HFCPCI_INTS_TIMER;
+		Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+		/* reinit mode reg */
+		Write_hfc(hc, HFCPCI_MST_MODE, hc->hw.mst_m);
+		if (!hc->irqcnt) {
+			printk(KERN_WARNING
+			    "HFC PCI: IRQ(%d) getting no interrupts "
+			    "during init %d\n", hc->irq, 4 - cnt);
+			if (cnt == 1) {
+				spin_unlock_irqrestore(&hc->lock, flags);
+				return -EIO;
+			} else {
+				reset_hfcpci(hc);
+				cnt--;
+			}
+		} else {
+			spin_unlock_irqrestore(&hc->lock, flags);
+			hc->initdone = 1;
+			return 0;
+		}
+	}
+	disable_hwirq(hc);
+	spin_unlock_irqrestore(&hc->lock, flags);
+	free_irq(hc->irq, hc);
+	return -EIO;
+}
+
+static int
+channel_ctrl(struct hfc_pci *hc, struct mISDN_ctrl_req *cq)
+{
+	int	ret = 0;
+	u_char	slot;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_CONNECT |
+		    MISDN_CTRL_DISCONNECT;
+		break;
+	case MISDN_CTRL_LOOP:
+		/* channel 0 disabled loop */
+		if (cq->channel < 0 || cq->channel > 2) {
+			ret = -EINVAL;
+			break;
+		}
+		if (cq->channel & 1) {
+			if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg))
+				slot = 0xC0;
+			else
+				slot = 0x80;
+			printk(KERN_DEBUG "%s: Write_hfc: B1_SSL/RSL 0x%x\n",
+			    __func__, slot);
+			Write_hfc(hc, HFCPCI_B1_SSL, slot);
+			Write_hfc(hc, HFCPCI_B1_RSL, slot);
+			hc->hw.conn = (hc->hw.conn & ~7) | 6;
+			Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+		}
+		if (cq->channel & 2) {
+			if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg))
+				slot = 0xC1;
+			else
+				slot = 0x81;
+			printk(KERN_DEBUG "%s: Write_hfc: B2_SSL/RSL 0x%x\n",
+			    __func__, slot);
+			Write_hfc(hc, HFCPCI_B2_SSL, slot);
+			Write_hfc(hc, HFCPCI_B2_RSL, slot);
+			hc->hw.conn = (hc->hw.conn & ~0x38) | 0x30;
+			Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+		}
+		if (cq->channel & 3)
+			hc->hw.trm |= 0x80;	/* enable IOM-loop */
+		else {
+			hc->hw.conn = (hc->hw.conn & ~0x3f) | 0x09;
+			Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+			hc->hw.trm &= 0x7f;	/* disable IOM-loop */
+		}
+		Write_hfc(hc, HFCPCI_TRM, hc->hw.trm);
+		break;
+	case MISDN_CTRL_CONNECT:
+		if (cq->channel == cq->p1) {
+			ret = -EINVAL;
+			break;
+		}
+		if (cq->channel < 1 || cq->channel > 2 ||
+		    cq->p1 < 1 || cq->p1 > 2) {
+			ret = -EINVAL;
+			break;
+		}
+		if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg))
+			slot = 0xC0;
+		else
+			slot = 0x80;
+		printk(KERN_DEBUG "%s: Write_hfc: B1_SSL/RSL 0x%x\n",
+		    __func__, slot);
+		Write_hfc(hc, HFCPCI_B1_SSL, slot);
+		Write_hfc(hc, HFCPCI_B2_RSL, slot);
+		if (test_bit(HFC_CFG_SW_DD_DU, &hc->cfg))
+			slot = 0xC1;
+		else
+			slot = 0x81;
+		printk(KERN_DEBUG "%s: Write_hfc: B2_SSL/RSL 0x%x\n",
+		    __func__, slot);
+		Write_hfc(hc, HFCPCI_B2_SSL, slot);
+		Write_hfc(hc, HFCPCI_B1_RSL, slot);
+		hc->hw.conn = (hc->hw.conn & ~0x3f) | 0x36;
+		Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+		hc->hw.trm |= 0x80;
+		Write_hfc(hc, HFCPCI_TRM, hc->hw.trm);
+		break;
+	case MISDN_CTRL_DISCONNECT:
+		hc->hw.conn = (hc->hw.conn & ~0x3f) | 0x09;
+		Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
+		hc->hw.trm &= 0x7f;	/* disable IOM-loop */
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n",
+		    __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int
+open_dchannel(struct hfc_pci *hc, struct mISDNchannel *ch,
+    struct channel_req *rq)
+{
+	int err = 0;
+
+	if (debug & DEBUG_HW_OPEN)
+		printk(KERN_DEBUG "%s: dev(%d) open from %p\n", __func__,
+		    hc->dch.dev.id, __builtin_return_address(0));
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	if (!hc->initdone) {
+		if (rq->protocol == ISDN_P_TE_S0) {
+			err = create_l1(&hc->dch, hfc_l1callback);
+			if (err)
+				return err;
+		}
+		hc->hw.protocol = rq->protocol;
+		ch->protocol = rq->protocol;
+		err = init_card(hc);
+		if (err)
+			return err;
+	} else {
+		if (rq->protocol != ch->protocol) {
+			if (hc->hw.protocol == ISDN_P_TE_S0)
+				l1_event(hc->dch.l1, CLOSE_CHANNEL);
+			hc->hw.protocol = rq->protocol;
+			ch->protocol = rq->protocol;
+			hfcpci_setmode(hc);
+		}
+	}
+
+	if (((ch->protocol == ISDN_P_NT_S0) && (hc->dch.state == 3)) ||
+	    ((ch->protocol == ISDN_P_TE_S0) && (hc->dch.state == 7))) {
+		_queue_data(ch, PH_ACTIVATE_IND, MISDN_ID_ANY,
+		    0, NULL, GFP_KERNEL);
+	}
+	rq->ch = ch;
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+static int
+open_bchannel(struct hfc_pci *hc, struct channel_req *rq)
+{
+	struct bchannel		*bch;
+
+	if (rq->adr.channel > 2)
+		return -EINVAL;
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	bch = &hc->bch[rq->adr.channel - 1];
+	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
+		return -EBUSY; /* b-channel can be only open once */
+	bch->ch.protocol = rq->protocol;
+	rq->ch = &bch->ch; /* TODO: E-channel */
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+/*
+ * device control function
+ */
+static int
+hfc_dctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct hfc_pci		*hc = dch->hw;
+	struct channel_req	*rq;
+	int			err = 0;
+
+	if (dch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n",
+		    __func__, cmd, arg);
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		rq = arg;
+		if (rq->adr.channel == 0)
+			err = open_dchannel(hc, ch, rq);
+		else
+			err = open_bchannel(hc, rq);
+		break;
+	case CLOSE_CHANNEL:
+		if (debug & DEBUG_HW_OPEN)
+			printk(KERN_DEBUG "%s: dev(%d) close from %p\n",
+			    __func__, hc->dch.dev.id,
+			    __builtin_return_address(0));
+		module_put(THIS_MODULE);
+		break;
+	case CONTROL_CHANNEL:
+		err = channel_ctrl(hc, arg);
+		break;
+	default:
+		if (dch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "%s: unknown command %x\n",
+			    __func__, cmd);
+		return -EINVAL;
+	}
+	return err;
+}
+
+static int
+setup_hw(struct hfc_pci *hc)
+{
+	void	*buffer;
+
+	printk(KERN_INFO "mISDN: HFC-PCI driver %s\n", hfcpci_revision);
+	hc->hw.cirm = 0;
+	hc->dch.state = 0;
+	pci_set_master(hc->pdev);
+	if (!hc->irq) {
+		printk(KERN_WARNING "HFC-PCI: No IRQ for PCI card found\n");
+		return 1;
+	}
+	hc->hw.pci_io = (char *)(ulong)hc->pdev->resource[1].start;
+
+	if (!hc->hw.pci_io) {
+		printk(KERN_WARNING "HFC-PCI: No IO-Mem for PCI card found\n");
+		return 1;
+	}
+	/* Allocate memory for FIFOS */
+	/* the memory needs to be on a 32k boundary within the first 4G */
+	pci_set_dma_mask(hc->pdev, 0xFFFF8000);
+	buffer = pci_alloc_consistent(hc->pdev, 0x8000, &hc->hw.dmahandle);
+	/* We silently assume the address is okay if nonzero */
+	if (!buffer) {
+		printk(KERN_WARNING
+		    "HFC-PCI: Error allocating memory for FIFO!\n");
+		return 1;
+	}
+	hc->hw.fifos = buffer;
+	pci_write_config_dword(hc->pdev, 0x80, hc->hw.dmahandle);
+	hc->hw.pci_io = ioremap((ulong) hc->hw.pci_io, 256);
+	printk(KERN_INFO
+		"HFC-PCI: defined at mem %#lx fifo %#lx(%#lx) IRQ %d HZ %d\n",
+		(u_long) hc->hw.pci_io, (u_long) hc->hw.fifos,
+		(u_long) virt_to_bus(hc->hw.fifos),
+		hc->irq, HZ);
+	/* enable memory mapped ports, disable busmaster */
+	pci_write_config_word(hc->pdev, PCI_COMMAND, PCI_ENA_MEMIO);
+	hc->hw.int_m2 = 0;
+	disable_hwirq(hc);
+	hc->hw.int_m1 = 0;
+	Write_hfc(hc, HFCPCI_INT_M1, hc->hw.int_m1);
+	/* At this point the needed PCI config is done */
+	/* fifos are still not enabled */
+	hc->hw.timer.function = (void *) hfcpci_Timer;
+	hc->hw.timer.data = (long) hc;
+	init_timer(&hc->hw.timer);
+	/* default PCM master */
+	test_and_set_bit(HFC_CFG_MASTER, &hc->cfg);
+	return 0;
+}
+
+static void
+release_card(struct hfc_pci *hc) {
+	u_long	flags;
+
+	spin_lock_irqsave(&hc->lock, flags);
+	hc->hw.int_m2 = 0; /* interrupt output off ! */
+	disable_hwirq(hc);
+	mode_hfcpci(&hc->bch[0], 1, ISDN_P_NONE);
+	mode_hfcpci(&hc->bch[1], 2, ISDN_P_NONE);
+	if (hc->dch.timer.function != NULL) {
+		del_timer(&hc->dch.timer);
+		hc->dch.timer.function = NULL;
+	}
+	spin_unlock_irqrestore(&hc->lock, flags);
+	if (hc->hw.protocol == ISDN_P_TE_S0)
+		l1_event(hc->dch.l1, CLOSE_CHANNEL);
+	if (hc->initdone)
+		free_irq(hc->irq, hc);
+	release_io_hfcpci(hc); /* must release after free_irq! */
+	mISDN_unregister_device(&hc->dch.dev);
+	mISDN_freebchannel(&hc->bch[1]);
+	mISDN_freebchannel(&hc->bch[0]);
+	mISDN_freedchannel(&hc->dch);
+	list_del(&hc->list);
+	pci_set_drvdata(hc->pdev, NULL);
+	kfree(hc);
+}
+
+static int
+setup_card(struct hfc_pci *card)
+{
+	int		err = -EINVAL;
+	u_int		i;
+	u_long		flags;
+	char		name[MISDN_MAX_IDLEN];
+
+	if (HFC_cnt >= MAX_CARDS)
+		return -EINVAL; /* maybe better value */
+
+	card->dch.debug = debug;
+	spin_lock_init(&card->lock);
+	mISDN_initdchannel(&card->dch, MAX_DFRAME_LEN_L1, ph_state);
+	card->dch.hw = card;
+	card->dch.dev.Dprotocols = (1 << ISDN_P_TE_S0) | (1 << ISDN_P_NT_S0);
+	card->dch.dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
+	    (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
+	card->dch.dev.D.send = hfcpci_l2l1D;
+	card->dch.dev.D.ctrl = hfc_dctrl;
+	card->dch.dev.nrbchan = 2;
+	for (i = 0; i < 2; i++) {
+		card->bch[i].nr = i + 1;
+		test_and_set_bit(i + 1, &card->dch.dev.channelmap[0]);
+		card->bch[i].debug = debug;
+		mISDN_initbchannel(&card->bch[i], MAX_DATA_MEM);
+		card->bch[i].hw = card;
+		card->bch[i].ch.send = hfcpci_l2l1B;
+		card->bch[i].ch.ctrl = hfc_bctrl;
+		card->bch[i].ch.nr = i + 1;
+		list_add(&card->bch[i].ch.list, &card->dch.dev.bchannels);
+	}
+	err = setup_hw(card);
+	if (err)
+		goto error;
+	snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-pci.%d", HFC_cnt + 1);
+	err = mISDN_register_device(&card->dch.dev, name);
+	if (err)
+		goto error;
+	HFC_cnt++;
+	write_lock_irqsave(&HFClock, flags);
+	list_add_tail(&card->list, &HFClist);
+	write_unlock_irqrestore(&HFClock, flags);
+	printk(KERN_INFO "HFC %d cards installed\n", HFC_cnt);
+	return 0;
+error:
+	mISDN_freebchannel(&card->bch[1]);
+	mISDN_freebchannel(&card->bch[0]);
+	mISDN_freedchannel(&card->dch);
+	kfree(card);
+	return err;
+}
+
+/* private data in the PCI devices list */
+struct _hfc_map {
+	u_int	subtype;
+	u_int	flag;
+	char	*name;
+};
+
+static const struct _hfc_map hfc_map[] =
+{
+	{HFC_CCD_2BD0, 0, "CCD/Billion/Asuscom 2BD0"},
+	{HFC_CCD_B000, 0, "Billion B000"},
+	{HFC_CCD_B006, 0, "Billion B006"},
+	{HFC_CCD_B007, 0, "Billion B007"},
+	{HFC_CCD_B008, 0, "Billion B008"},
+	{HFC_CCD_B009, 0, "Billion B009"},
+	{HFC_CCD_B00A, 0, "Billion B00A"},
+	{HFC_CCD_B00B, 0, "Billion B00B"},
+	{HFC_CCD_B00C, 0, "Billion B00C"},
+	{HFC_CCD_B100, 0, "Seyeon B100"},
+	{HFC_CCD_B700, 0, "Primux II S0 B700"},
+	{HFC_CCD_B701, 0, "Primux II S0 NT B701"},
+	{HFC_ABOCOM_2BD1, 0, "Abocom/Magitek 2BD1"},
+	{HFC_ASUS_0675, 0, "Asuscom/Askey 675"},
+	{HFC_BERKOM_TCONCEPT, 0, "German telekom T-Concept"},
+	{HFC_BERKOM_A1T, 0, "German telekom A1T"},
+	{HFC_ANIGMA_MC145575, 0, "Motorola MC145575"},
+	{HFC_ZOLTRIX_2BD0, 0, "Zoltrix 2BD0"},
+	{HFC_DIGI_DF_M_IOM2_E, 0,
+	    "Digi International DataFire Micro V IOM2 (Europe)"},
+	{HFC_DIGI_DF_M_E, 0,
+	    "Digi International DataFire Micro V (Europe)"},
+	{HFC_DIGI_DF_M_IOM2_A, 0,
+	    "Digi International DataFire Micro V IOM2 (North America)"},
+	{HFC_DIGI_DF_M_A, 0,
+	    "Digi International DataFire Micro V (North America)"},
+	{HFC_SITECOM_DC105V2, 0, "Sitecom Connectivity DC-105 ISDN TA"},
+	{},
+};
+
+static struct pci_device_id hfc_ids[] =
+{
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_2BD0,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[0]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B000,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[1]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B006,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[2]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B007,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[3]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B008,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[4]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B009,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[5]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B00A,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[6]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B00B,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[7]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B00C,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[8]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B100,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[9]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B700,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[10]},
+	{PCI_VENDOR_ID_CCD, PCI_DEVICE_ID_CCD_B701,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[11]},
+	{PCI_VENDOR_ID_ABOCOM, PCI_DEVICE_ID_ABOCOM_2BD1,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[12]},
+	{PCI_VENDOR_ID_ASUSTEK, PCI_DEVICE_ID_ASUSTEK_0675,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[13]},
+	{PCI_VENDOR_ID_BERKOM, PCI_DEVICE_ID_BERKOM_T_CONCEPT,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[14]},
+	{PCI_VENDOR_ID_BERKOM, PCI_DEVICE_ID_BERKOM_A1T,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[15]},
+	{PCI_VENDOR_ID_ANIGMA, PCI_DEVICE_ID_ANIGMA_MC145575,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[16]},
+	{PCI_VENDOR_ID_ZOLTRIX, PCI_DEVICE_ID_ZOLTRIX_2BD0,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[17]},
+	{PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_DIGI_DF_M_IOM2_E,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[18]},
+	{PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_DIGI_DF_M_E,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[19]},
+	{PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_DIGI_DF_M_IOM2_A,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[20]},
+	{PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_DIGI_DF_M_A,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[21]},
+	{PCI_VENDOR_ID_SITECOM, PCI_DEVICE_ID_SITECOM_DC105V2,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0, (unsigned long) &hfc_map[22]},
+	{},
+};
+
+static int __devinit
+hfc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int		err = -ENOMEM;
+	struct hfc_pci	*card;
+	struct _hfc_map	*m = (struct _hfc_map *)ent->driver_data;
+
+	card = kzalloc(sizeof(struct hfc_pci), GFP_ATOMIC);
+	if (!card) {
+		printk(KERN_ERR "No kmem for HFC card\n");
+		return err;
+	}
+	card->pdev = pdev;
+	card->subtype = m->subtype;
+	err = pci_enable_device(pdev);
+	if (err) {
+		kfree(card);
+		return err;
+	}
+
+	printk(KERN_INFO "mISDN_hfcpci: found adapter %s at %s\n",
+	       m->name, pci_name(pdev));
+
+	card->irq = pdev->irq;
+	pci_set_drvdata(pdev, card);
+	err = setup_card(card);
+	if (err)
+		pci_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static void __devexit
+hfc_remove_pci(struct pci_dev *pdev)
+{
+	struct hfc_pci	*card = pci_get_drvdata(pdev);
+	u_long		flags;
+
+	if (card) {
+		write_lock_irqsave(&HFClock, flags);
+		release_card(card);
+		write_unlock_irqrestore(&HFClock, flags);
+	} else
+		if (debug)
+			printk(KERN_WARNING "%s: drvdata allready removed\n",
+			    __func__);
+}
+
+
+static struct pci_driver hfc_driver = {
+	.name = "hfcpci",
+	.probe = hfc_probe,
+	.remove = __devexit_p(hfc_remove_pci),
+	.id_table = hfc_ids,
+};
+
+static int __init
+HFC_init(void)
+{
+	int		err;
+
+	err = pci_register_driver(&hfc_driver);
+	return err;
+}
+
+static void __exit
+HFC_cleanup(void)
+{
+	struct hfc_pci	*card, *next;
+
+	list_for_each_entry_safe(card, next, &HFClist, list) {
+		release_card(card);
+	}
+	pci_unregister_driver(&hfc_driver);
+}
+
+module_init(HFC_init);
+module_exit(HFC_cleanup);
diff --git a/drivers/isdn/hisax/hisax_fcpcipnp.c b/drivers/isdn/hisax/hisax_fcpcipnp.c
index c0b4db2f836..1925118122f 100644
--- a/drivers/isdn/hisax/hisax_fcpcipnp.c
+++ b/drivers/isdn/hisax/hisax_fcpcipnp.c
@@ -974,8 +974,6 @@ static struct pnp_driver fcpnp_driver = {
 	.remove		= __devexit_p(fcpnp_remove),
 	.id_table	= fcpnp_ids,
 };
-#else
-static struct pnp_driver fcpnp_driver;
 #endif
 
 static void __devexit fcpci_remove(struct pci_dev *pdev)
diff --git a/drivers/isdn/hisax/st5481.h b/drivers/isdn/hisax/st5481.h
index 2044e7173ab..cff7a635433 100644
--- a/drivers/isdn/hisax/st5481.h
+++ b/drivers/isdn/hisax/st5481.h
@@ -220,7 +220,7 @@ enum {
 #define ERR(format, arg...) \
 printk(KERN_ERR "%s:%s: " format "\n" , __FILE__,  __func__ , ## arg)
 
-#define WARN(format, arg...) \
+#define WARNING(format, arg...) \
 printk(KERN_WARNING "%s:%s: " format "\n" , __FILE__,  __func__ , ## arg)
 
 #define INFO(format, arg...) \
@@ -412,7 +412,7 @@ struct st5481_adapter {
 ({ \
 	int status; \
 	if ((status = usb_submit_urb(urb, mem_flags)) < 0) { \
-		WARN("usb_submit_urb failed,status=%d", status); \
+		WARNING("usb_submit_urb failed,status=%d", status); \
 	} \
         status; \
 })
diff --git a/drivers/isdn/hisax/st5481_b.c b/drivers/isdn/hisax/st5481_b.c
index fa64115cd7c..0074b600a0e 100644
--- a/drivers/isdn/hisax/st5481_b.c
+++ b/drivers/isdn/hisax/st5481_b.c
@@ -180,7 +180,7 @@ static void usb_b_out_complete(struct urb *urb)
 				DBG(4,"urb killed status %d", urb->status);
 				return; // Give up
 			default: 
-				WARN("urb status %d",urb->status);
+				WARNING("urb status %d",urb->status);
 				if (b_out->busy == 0) {
 					st5481_usb_pipe_reset(adapter, (bcs->channel+1)*2 | USB_DIR_OUT, NULL, NULL);
 				}
@@ -372,6 +372,6 @@ void st5481_b_l2l1(struct hisax_if *ifc, int pr, void *arg)
 		B_L1L2(bcs, PH_DEACTIVATE | INDICATION, NULL);
 		break;
 	default:
-		WARN("pr %#x\n", pr);
+		WARNING("pr %#x\n", pr);
 	}
 }
diff --git a/drivers/isdn/hisax/st5481_d.c b/drivers/isdn/hisax/st5481_d.c
index b8c4855cc88..077991c1cd0 100644
--- a/drivers/isdn/hisax/st5481_d.c
+++ b/drivers/isdn/hisax/st5481_d.c
@@ -389,7 +389,7 @@ static void usb_d_out_complete(struct urb *urb)
 				DBG(1,"urb killed status %d", urb->status);
 				break;
 			default: 
-				WARN("urb status %d",urb->status);
+				WARNING("urb status %d",urb->status);
 				if (d_out->busy == 0) {
 					st5481_usb_pipe_reset(adapter, EP_D_OUT | USB_DIR_OUT, fifo_reseted, adapter);
 				}
@@ -420,7 +420,7 @@ static void dout_start_xmit(struct FsmInst *fsm, int event, void *arg)
 	isdnhdlc_out_init(&d_out->hdlc_state, 1, 0);
 
 	if (test_and_set_bit(buf_nr, &d_out->busy)) {
-		WARN("ep %d urb %d busy %#lx", EP_D_OUT, buf_nr, d_out->busy);
+		WARNING("ep %d urb %d busy %#lx", EP_D_OUT, buf_nr, d_out->busy);
 		return;
 	}
 	urb = d_out->urb[buf_nr];
@@ -601,7 +601,7 @@ void st5481_d_l2l1(struct hisax_if *hisax_d_if, int pr, void *arg)
 		FsmEvent(&adapter->d_out.fsm, EV_DOUT_START_XMIT, NULL);
 		break;
 	default:
-		WARN("pr %#x\n", pr);
+		WARNING("pr %#x\n", pr);
 		break;
 	}
 }
diff --git a/drivers/isdn/hisax/st5481_usb.c b/drivers/isdn/hisax/st5481_usb.c
index 427a8b0520f..ec3c0e50766 100644
--- a/drivers/isdn/hisax/st5481_usb.c
+++ b/drivers/isdn/hisax/st5481_usb.c
@@ -66,7 +66,7 @@ static void usb_ctrl_msg(struct st5481_adapter *adapter,
 	struct ctrl_msg *ctrl_msg;
 	
 	if ((w_index = fifo_add(&ctrl->msg_fifo.f)) < 0) {
-		WARN("control msg FIFO full");
+		WARNING("control msg FIFO full");
 		return;
 	}
 	ctrl_msg = &ctrl->msg_fifo.data[w_index]; 
@@ -139,7 +139,7 @@ static void usb_ctrl_complete(struct urb *urb)
 				DBG(1,"urb killed status %d", urb->status);
 				return; // Give up
 			default: 
-				WARN("urb status %d",urb->status);
+				WARNING("urb status %d",urb->status);
 				break;
 		}
 	}
@@ -198,7 +198,7 @@ static void usb_int_complete(struct urb *urb)
 			DBG(2, "urb shutting down with status: %d", urb->status);
 			return;
 		default:
-			WARN("nonzero urb status received: %d", urb->status);
+			WARNING("nonzero urb status received: %d", urb->status);
 			goto exit;
 	}
 
@@ -235,7 +235,7 @@ static void usb_int_complete(struct urb *urb)
 exit:
 	status = usb_submit_urb (urb, GFP_ATOMIC);
 	if (status)
-		WARN("usb_submit_urb failed with result %d", status);
+		WARNING("usb_submit_urb failed with result %d", status);
 }
 
 /* ======================================================================
@@ -257,7 +257,7 @@ int st5481_setup_usb(struct st5481_adapter *adapter)
 	DBG(2,"");
 	
 	if ((status = usb_reset_configuration (dev)) < 0) {
-		WARN("reset_configuration failed,status=%d",status);
+		WARNING("reset_configuration failed,status=%d",status);
 		return status;
 	}
 
@@ -269,7 +269,7 @@ int st5481_setup_usb(struct st5481_adapter *adapter)
 
 	// Check if the config is sane
 	if ( altsetting->desc.bNumEndpoints != 7 ) {
-		WARN("expecting 7 got %d endpoints!", altsetting->desc.bNumEndpoints);
+		WARNING("expecting 7 got %d endpoints!", altsetting->desc.bNumEndpoints);
 		return -EINVAL;
 	}
 
@@ -279,7 +279,7 @@ int st5481_setup_usb(struct st5481_adapter *adapter)
 
 	// Use alternative setting 3 on interface 0 to have 2B+D
 	if ((status = usb_set_interface (dev, 0, 3)) < 0) {
-		WARN("usb_set_interface failed,status=%d",status);
+		WARNING("usb_set_interface failed,status=%d",status);
 		return status;
 	}
 
@@ -497,7 +497,7 @@ static void usb_in_complete(struct urb *urb)
 				DBG(1,"urb killed status %d", urb->status);
 				return; // Give up
 			default: 
-				WARN("urb status %d",urb->status);
+				WARNING("urb status %d",urb->status);
 				break;
 		}
 	}
@@ -523,7 +523,7 @@ static void usb_in_complete(struct urb *urb)
 			DBG(4,"count=%d",status);
 			DBG_PACKET(0x400, in->rcvbuf, status);
 			if (!(skb = dev_alloc_skb(status))) {
-				WARN("receive out of memory\n");
+				WARNING("receive out of memory\n");
 				break;
 			}
 			memcpy(skb_put(skb, status), in->rcvbuf, status);
diff --git a/drivers/isdn/mISDN/Kconfig b/drivers/isdn/mISDN/Kconfig
new file mode 100644
index 00000000000..4938355c407
--- /dev/null
+++ b/drivers/isdn/mISDN/Kconfig
@@ -0,0 +1,44 @@
+#
+# modularer ISDN driver
+#
+
+menuconfig MISDN
+	tristate "Modular ISDN driver"
+	help
+	  Enable support for the modular ISDN driver.
+
+if MISDN != n
+
+config MISDN_DSP
+	tristate "Digital Audio Processing of transparent data"
+	depends on MISDN
+	help
+	  Enable support for digital audio processing capability.
+	  This module may be used for special applications that require
+	  cross connecting of bchannels, conferencing, dtmf decoding
+	  echo cancelation, tone generation, and Blowfish encryption and
+	  decryption.
+	  It may use hardware features if available.
+	  E.g. it is required for PBX4Linux. Go to http://isdn.eversberg.eu
+	  and get more informations about this module and it's usage.
+	  If unsure, say 'N'.
+
+config MISDN_L1OIP
+	tristate "ISDN over IP tunnel"
+	depends on MISDN
+	help
+	  Enable support for ISDN over IP tunnel.
+
+	  It features:
+	    - dynamic IP exchange, if one or both peers have dynamic IPs
+	    - BRI (S0) and PRI (S2M) interface
+	    - layer 1 control via network keepalive frames
+	    - direct tunneling of physical interface via IP
+
+	  NOTE: This protocol is called 'Layer 1 over IP' and is not
+	  compatible with ISDNoIP (Agfeo) or TDMoIP. Protocol description is
+	  provided in the source code.
+
+source "drivers/isdn/hardware/mISDN/Kconfig"
+
+endif #MISDN
diff --git a/drivers/isdn/mISDN/Makefile b/drivers/isdn/mISDN/Makefile
new file mode 100644
index 00000000000..1cb5e633cf7
--- /dev/null
+++ b/drivers/isdn/mISDN/Makefile
@@ -0,0 +1,13 @@
+#
+# Makefile for the modular ISDN driver
+#
+
+obj-$(CONFIG_MISDN) += mISDN_core.o
+obj-$(CONFIG_MISDN_DSP) += mISDN_dsp.o
+obj-$(CONFIG_MISDN_L1OIP) += l1oip.o
+
+# multi objects
+
+mISDN_core-objs := core.o fsm.o socket.o hwchannel.o stack.o layer1.o layer2.o tei.o timerdev.o
+mISDN_dsp-objs := dsp_core.o dsp_cmx.o dsp_tones.o dsp_dtmf.o dsp_audio.o dsp_blowfish.o dsp_pipeline.o dsp_hwec.o
+l1oip-objs := l1oip_core.o l1oip_codec.o
diff --git a/drivers/isdn/mISDN/core.c b/drivers/isdn/mISDN/core.c
new file mode 100644
index 00000000000..33068177b7c
--- /dev/null
+++ b/drivers/isdn/mISDN/core.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/stddef.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/mISDNif.h>
+#include "core.h"
+
+static u_int debug;
+
+MODULE_AUTHOR("Karsten Keil");
+MODULE_LICENSE("GPL");
+module_param(debug, uint, S_IRUGO | S_IWUSR);
+
+static LIST_HEAD(devices);
+DEFINE_RWLOCK(device_lock);
+static u64		device_ids;
+#define MAX_DEVICE_ID	63
+
+static LIST_HEAD(Bprotocols);
+DEFINE_RWLOCK(bp_lock);
+
+struct mISDNdevice
+*get_mdevice(u_int id)
+{
+	struct mISDNdevice	*dev;
+
+	read_lock(&device_lock);
+	list_for_each_entry(dev, &devices, D.list)
+		if (dev->id == id) {
+			read_unlock(&device_lock);
+			return dev;
+		}
+	read_unlock(&device_lock);
+	return NULL;
+}
+
+int
+get_mdevice_count(void)
+{
+	struct mISDNdevice	*dev;
+	int			cnt = 0;
+
+	read_lock(&device_lock);
+	list_for_each_entry(dev, &devices, D.list)
+		cnt++;
+	read_unlock(&device_lock);
+	return cnt;
+}
+
+static int
+get_free_devid(void)
+{
+	u_int	i;
+
+	for (i = 0; i <= MAX_DEVICE_ID; i++)
+		if (!test_and_set_bit(i, (u_long *)&device_ids))
+			return i;
+	return -1;
+}
+
+int
+mISDN_register_device(struct mISDNdevice *dev, char *name)
+{
+	u_long	flags;
+	int	err;
+
+	dev->id = get_free_devid();
+	if (dev->id < 0)
+		return -EBUSY;
+	if (name && name[0])
+		strcpy(dev->name, name);
+	else
+		sprintf(dev->name, "mISDN%d", dev->id);
+	if (debug & DEBUG_CORE)
+		printk(KERN_DEBUG "mISDN_register %s %d\n",
+			dev->name, dev->id);
+	err = create_stack(dev);
+	if (err)
+		return err;
+	write_lock_irqsave(&device_lock, flags);
+	list_add_tail(&dev->D.list, &devices);
+	write_unlock_irqrestore(&device_lock, flags);
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_register_device);
+
+void
+mISDN_unregister_device(struct mISDNdevice *dev) {
+	u_long	flags;
+
+	if (debug & DEBUG_CORE)
+		printk(KERN_DEBUG "mISDN_unregister %s %d\n",
+			dev->name, dev->id);
+	write_lock_irqsave(&device_lock, flags);
+	list_del(&dev->D.list);
+	write_unlock_irqrestore(&device_lock, flags);
+	test_and_clear_bit(dev->id, (u_long *)&device_ids);
+	delete_stack(dev);
+}
+EXPORT_SYMBOL(mISDN_unregister_device);
+
+u_int
+get_all_Bprotocols(void)
+{
+	struct Bprotocol	*bp;
+	u_int	m = 0;
+
+	read_lock(&bp_lock);
+	list_for_each_entry(bp, &Bprotocols, list)
+		m |= bp->Bprotocols;
+	read_unlock(&bp_lock);
+	return m;
+}
+
+struct Bprotocol *
+get_Bprotocol4mask(u_int m)
+{
+	struct Bprotocol	*bp;
+
+	read_lock(&bp_lock);
+	list_for_each_entry(bp, &Bprotocols, list)
+		if (bp->Bprotocols & m) {
+			read_unlock(&bp_lock);
+			return bp;
+		}
+	read_unlock(&bp_lock);
+	return NULL;
+}
+
+struct Bprotocol *
+get_Bprotocol4id(u_int id)
+{
+	u_int	m;
+
+	if (id < ISDN_P_B_START || id > 63) {
+		printk(KERN_WARNING "%s id not in range  %d\n",
+		    __func__, id);
+		return NULL;
+	}
+	m = 1 << (id & ISDN_P_B_MASK);
+	return get_Bprotocol4mask(m);
+}
+
+int
+mISDN_register_Bprotocol(struct Bprotocol *bp)
+{
+	u_long			flags;
+	struct Bprotocol	*old;
+
+	if (debug & DEBUG_CORE)
+		printk(KERN_DEBUG "%s: %s/%x\n", __func__,
+		    bp->name, bp->Bprotocols);
+	old = get_Bprotocol4mask(bp->Bprotocols);
+	if (old) {
+		printk(KERN_WARNING
+		    "register duplicate protocol old %s/%x new %s/%x\n",
+		    old->name, old->Bprotocols, bp->name, bp->Bprotocols);
+		return -EBUSY;
+	}
+	write_lock_irqsave(&bp_lock, flags);
+	list_add_tail(&bp->list, &Bprotocols);
+	write_unlock_irqrestore(&bp_lock, flags);
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_register_Bprotocol);
+
+void
+mISDN_unregister_Bprotocol(struct Bprotocol *bp)
+{
+	u_long	flags;
+
+	if (debug & DEBUG_CORE)
+		printk(KERN_DEBUG "%s: %s/%x\n", __func__, bp->name,
+			bp->Bprotocols);
+	write_lock_irqsave(&bp_lock, flags);
+	list_del(&bp->list);
+	write_unlock_irqrestore(&bp_lock, flags);
+}
+EXPORT_SYMBOL(mISDN_unregister_Bprotocol);
+
+int
+mISDNInit(void)
+{
+	int	err;
+
+	printk(KERN_INFO "Modular ISDN core version %d.%d.%d\n",
+		MISDN_MAJOR_VERSION, MISDN_MINOR_VERSION, MISDN_RELEASE);
+	mISDN_initstack(&debug);
+	err = mISDN_inittimer(&debug);
+	if (err)
+		goto error;
+	err = l1_init(&debug);
+	if (err) {
+		mISDN_timer_cleanup();
+		goto error;
+	}
+	err = Isdnl2_Init(&debug);
+	if (err) {
+		mISDN_timer_cleanup();
+		l1_cleanup();
+		goto error;
+	}
+	err = misdn_sock_init(&debug);
+	if (err) {
+		mISDN_timer_cleanup();
+		l1_cleanup();
+		Isdnl2_cleanup();
+	}
+error:
+	return err;
+}
+
+void mISDN_cleanup(void)
+{
+	misdn_sock_cleanup();
+	mISDN_timer_cleanup();
+	l1_cleanup();
+	Isdnl2_cleanup();
+
+	if (!list_empty(&devices))
+		printk(KERN_ERR "%s devices still registered\n", __func__);
+
+	if (!list_empty(&Bprotocols))
+		printk(KERN_ERR "%s Bprotocols still registered\n", __func__);
+	printk(KERN_DEBUG "mISDNcore unloaded\n");
+}
+
+module_init(mISDNInit);
+module_exit(mISDN_cleanup);
+
diff --git a/drivers/isdn/mISDN/core.h b/drivers/isdn/mISDN/core.h
new file mode 100644
index 00000000000..7da7233b4c1
--- /dev/null
+++ b/drivers/isdn/mISDN/core.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef mISDN_CORE_H
+#define mISDN_CORE_H
+
+extern struct mISDNdevice	*get_mdevice(u_int);
+extern int			get_mdevice_count(void);
+
+/* stack status flag */
+#define mISDN_STACK_ACTION_MASK		0x0000ffff
+#define mISDN_STACK_COMMAND_MASK	0x000f0000
+#define mISDN_STACK_STATUS_MASK		0xfff00000
+/* action bits 0-15 */
+#define mISDN_STACK_WORK	0
+#define mISDN_STACK_SETUP	1
+#define mISDN_STACK_CLEARING	2
+#define mISDN_STACK_RESTART	3
+#define mISDN_STACK_WAKEUP	4
+#define mISDN_STACK_ABORT	15
+/* command bits 16-19 */
+#define mISDN_STACK_STOPPED	16
+#define mISDN_STACK_INIT	17
+#define mISDN_STACK_THREADSTART	18
+/* status bits 20-31 */
+#define mISDN_STACK_BCHANNEL	20
+#define mISDN_STACK_ACTIVE      29
+#define mISDN_STACK_RUNNING     30
+#define mISDN_STACK_KILLED      31
+
+
+/* manager options */
+#define MGR_OPT_USER		24
+#define MGR_OPT_NETWORK		25
+
+extern int	connect_Bstack(struct mISDNdevice *, struct mISDNchannel *,
+			u_int, struct sockaddr_mISDN *);
+extern int	connect_layer1(struct mISDNdevice *, struct mISDNchannel *,
+			u_int, struct sockaddr_mISDN *);
+extern int	create_l2entity(struct mISDNdevice *, struct mISDNchannel *,
+			u_int, struct sockaddr_mISDN *);
+
+extern int	create_stack(struct mISDNdevice *);
+extern int	create_teimanager(struct mISDNdevice *);
+extern void	delete_teimanager(struct mISDNchannel *);
+extern void	delete_channel(struct mISDNchannel *);
+extern void	delete_stack(struct mISDNdevice *);
+extern void	mISDN_initstack(u_int *);
+extern int      misdn_sock_init(u_int *);
+extern void     misdn_sock_cleanup(void);
+extern void	add_layer2(struct mISDNchannel *, struct mISDNstack *);
+extern void	__add_layer2(struct mISDNchannel *, struct mISDNstack *);
+
+extern u_int		get_all_Bprotocols(void);
+struct Bprotocol	*get_Bprotocol4mask(u_int);
+struct Bprotocol	*get_Bprotocol4id(u_int);
+
+extern int	mISDN_inittimer(u_int *);
+extern void	mISDN_timer_cleanup(void);
+
+extern int	l1_init(u_int *);
+extern void	l1_cleanup(void);
+extern int 	Isdnl2_Init(u_int *);
+extern void	Isdnl2_cleanup(void);
+
+#endif
diff --git a/drivers/isdn/mISDN/dsp.h b/drivers/isdn/mISDN/dsp.h
new file mode 100644
index 00000000000..6c3fed6b8d4
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp.h
@@ -0,0 +1,263 @@
+/*
+ * Audio support data for ISDN4Linux.
+ *
+ * Copyright 2002/2003 by Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+#define DEBUG_DSP_CTRL		0x0001
+#define DEBUG_DSP_CORE		0x0002
+#define DEBUG_DSP_DTMF		0x0004
+#define DEBUG_DSP_CMX		0x0010
+#define DEBUG_DSP_TONE		0x0020
+#define DEBUG_DSP_BLOWFISH	0x0040
+#define DEBUG_DSP_DELAY		0x0100
+#define DEBUG_DSP_DTMFCOEFF	0x8000 /* heavy output */
+
+/* options may be:
+ *
+ * bit 0 = use ulaw instead of alaw
+ * bit 1 = enable hfc hardware accelleration for all channels
+ *
+ */
+#define DSP_OPT_ULAW		(1<<0)
+#define DSP_OPT_NOHARDWARE	(1<<1)
+
+#include <linux/timer.h>
+#include <linux/workqueue.h>
+
+#include "dsp_ecdis.h"
+
+extern int dsp_options;
+extern int dsp_debug;
+extern int dsp_poll;
+extern int dsp_tics;
+extern spinlock_t dsp_lock;
+extern struct work_struct dsp_workq;
+extern u32 dsp_poll_diff; /* calculated fix-comma corrected poll value */
+
+/***************
+ * audio stuff *
+ ***************/
+
+extern s32 dsp_audio_alaw_to_s32[256];
+extern s32 dsp_audio_ulaw_to_s32[256];
+extern s32 *dsp_audio_law_to_s32;
+extern u8 dsp_audio_s16_to_law[65536];
+extern u8 dsp_audio_alaw_to_ulaw[256];
+extern u8 dsp_audio_mix_law[65536];
+extern u8 dsp_audio_seven2law[128];
+extern u8 dsp_audio_law2seven[256];
+extern void dsp_audio_generate_law_tables(void);
+extern void dsp_audio_generate_s2law_table(void);
+extern void dsp_audio_generate_seven(void);
+extern void dsp_audio_generate_mix_table(void);
+extern void dsp_audio_generate_ulaw_samples(void);
+extern void dsp_audio_generate_volume_changes(void);
+extern u8 dsp_silence;
+
+
+/*************
+ * cmx stuff *
+ *************/
+
+#define MAX_POLL	256	/* maximum number of send-chunks */
+
+#define CMX_BUFF_SIZE	0x8000	/* must be 2**n (0x1000 about 1/2 second) */
+#define CMX_BUFF_HALF	0x4000	/* CMX_BUFF_SIZE / 2 */
+#define CMX_BUFF_MASK	0x7fff	/* CMX_BUFF_SIZE - 1 */
+
+/* how many seconds will we check the lowest delay until the jitter buffer
+   is reduced by that delay */
+#define MAX_SECONDS_JITTER_CHECK 5
+
+extern struct timer_list dsp_spl_tl;
+extern u32 dsp_spl_jiffies;
+
+/* the structure of conferences:
+ *
+ * each conference has a unique number, given by user space.
+ * the conferences are linked in a chain.
+ * each conference has members linked in a chain.
+ * each dsplayer points to a member, each member points to a dsplayer.
+ */
+
+/* all members within a conference (this is linked 1:1 with the dsp) */
+struct dsp;
+struct dsp_conf_member {
+	struct list_head	list;
+	struct dsp		*dsp;
+};
+
+/* the list of all conferences */
+struct dsp_conf {
+	struct list_head	list;
+	u32			id;
+				/* all cmx stacks with the same ID are
+				 connected */
+	struct list_head	mlist;
+	int			software; /* conf is processed by software */
+	int			hardware; /* conf is processed by hardware */
+				/* note: if both unset, has only one member */
+};
+
+
+/**************
+ * DTMF stuff *
+ **************/
+
+#define DSP_DTMF_NPOINTS 102
+
+#define ECHOCAN_BUFLEN (4*128)
+
+struct dsp_dtmf {
+	int		treshold; /* above this is dtmf (square of) */
+	int		software; /* dtmf uses software decoding */
+	int		hardware; /* dtmf uses hardware decoding */
+	int		size; /* number of bytes in buffer */
+	signed short	buffer[DSP_DTMF_NPOINTS];
+		/* buffers one full dtmf frame */
+	u8		lastwhat, lastdigit;
+	int		count;
+	u8		digits[16]; /* just the dtmf result */
+};
+
+
+/******************
+ * pipeline stuff *
+ ******************/
+struct dsp_pipeline {
+	rwlock_t  lock;
+	struct list_head list;
+	int inuse;
+};
+
+/***************
+ * tones stuff *
+ ***************/
+
+struct dsp_tone {
+	int		software; /* tones are generated by software */
+	int		hardware; /* tones are generated by hardware */
+	int		tone;
+	void		*pattern;
+	int		count;
+	int		index;
+	struct timer_list tl;
+};
+
+/*****************
+ * general stuff *
+ *****************/
+
+struct dsp {
+	struct list_head list;
+	struct mISDNchannel	ch;
+	struct mISDNchannel	*up;
+	unsigned char	name[64];
+	int		b_active;
+	int		echo; /* echo is enabled */
+	int		rx_disabled; /* what the user wants */
+	int		rx_is_off; /* what the card is */
+	int		tx_mix;
+	struct dsp_tone	tone;
+	struct dsp_dtmf	dtmf;
+	int		tx_volume, rx_volume;
+
+	/* queue for sending frames */
+	struct work_struct	workq;
+	struct sk_buff_head	sendq;
+	int		hdlc;	/* if mode is hdlc */
+	int		data_pending;	/* currently an unconfirmed frame */
+
+	/* conference stuff */
+	u32		conf_id;
+	struct dsp_conf	*conf;
+	struct dsp_conf_member
+			*member;
+
+	/* buffer stuff */
+	int		rx_W; /* current write pos for data without timestamp */
+	int		rx_R; /* current read pos for transmit clock */
+	int		rx_init; /* if set, pointers will be adjusted first */
+	int		tx_W; /* current write pos for transmit data */
+	int		tx_R; /* current read pos for transmit clock */
+	int		rx_delay[MAX_SECONDS_JITTER_CHECK];
+	int		tx_delay[MAX_SECONDS_JITTER_CHECK];
+	u8		tx_buff[CMX_BUFF_SIZE];
+	u8		rx_buff[CMX_BUFF_SIZE];
+	int		last_tx; /* if set, we transmitted last poll interval */
+	int		cmx_delay; /* initial delay of buffers,
+				or 0 for dynamic jitter buffer */
+	int		tx_dejitter; /* if set, dejitter tx buffer */
+	int		tx_data; /* enables tx-data of CMX to upper layer */
+
+	/* hardware stuff */
+	struct dsp_features features;
+	int		features_rx_off; /* set if rx_off is featured */
+	int		pcm_slot_rx; /* current PCM slot (or -1) */
+	int		pcm_bank_rx;
+	int		pcm_slot_tx;
+	int		pcm_bank_tx;
+	int		hfc_conf; /* unique id of current conference (or -1) */
+
+	/* encryption stuff */
+	int		bf_enable;
+	u32		bf_p[18];
+	u32		bf_s[1024];
+	int		bf_crypt_pos;
+	u8		bf_data_in[9];
+	u8		bf_crypt_out[9];
+	int		bf_decrypt_in_pos;
+	int		bf_decrypt_out_pos;
+	u8		bf_crypt_inring[16];
+	u8		bf_data_out[9];
+	int		bf_sync;
+
+	struct dsp_pipeline
+			pipeline;
+};
+
+/* functions */
+
+extern void dsp_change_volume(struct sk_buff *skb, int volume);
+
+extern struct list_head dsp_ilist;
+extern struct list_head conf_ilist;
+extern void dsp_cmx_debug(struct dsp *dsp);
+extern void dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp);
+extern int dsp_cmx_conf(struct dsp *dsp, u32 conf_id);
+extern void dsp_cmx_receive(struct dsp *dsp, struct sk_buff *skb);
+extern void dsp_cmx_hdlc(struct dsp *dsp, struct sk_buff *skb);
+extern void dsp_cmx_send(void *arg);
+extern void dsp_cmx_transmit(struct dsp *dsp, struct sk_buff *skb);
+extern int dsp_cmx_del_conf_member(struct dsp *dsp);
+extern int dsp_cmx_del_conf(struct dsp_conf *conf);
+
+extern void dsp_dtmf_goertzel_init(struct dsp *dsp);
+extern void dsp_dtmf_hardware(struct dsp *dsp);
+extern u8 *dsp_dtmf_goertzel_decode(struct dsp *dsp, u8 *data, int len,
+		int fmt);
+
+extern int dsp_tone(struct dsp *dsp, int tone);
+extern void dsp_tone_copy(struct dsp *dsp, u8 *data, int len);
+extern void dsp_tone_timeout(void *arg);
+
+extern void dsp_bf_encrypt(struct dsp *dsp, u8 *data, int len);
+extern void dsp_bf_decrypt(struct dsp *dsp, u8 *data, int len);
+extern int dsp_bf_init(struct dsp *dsp, const u8 *key, unsigned int keylen);
+extern void dsp_bf_cleanup(struct dsp *dsp);
+
+extern int  dsp_pipeline_module_init(void);
+extern void dsp_pipeline_module_exit(void);
+extern int  dsp_pipeline_init(struct dsp_pipeline *pipeline);
+extern void dsp_pipeline_destroy(struct dsp_pipeline *pipeline);
+extern int  dsp_pipeline_build(struct dsp_pipeline *pipeline, const char *cfg);
+extern void dsp_pipeline_process_tx(struct dsp_pipeline *pipeline, u8 *data,
+		int len);
+extern void dsp_pipeline_process_rx(struct dsp_pipeline *pipeline, u8 *data,
+		int len);
+
diff --git a/drivers/isdn/mISDN/dsp_audio.c b/drivers/isdn/mISDN/dsp_audio.c
new file mode 100644
index 00000000000..1c2dd569477
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_audio.c
@@ -0,0 +1,434 @@
+/*
+ * Audio support data for mISDN_dsp.
+ *
+ * Copyright 2002/2003 by Andreas Eversberg (jolly@eversberg.eu)
+ * Rewritten by Peter
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "core.h"
+#include "dsp.h"
+
+/* ulaw[unsigned char] -> signed 16-bit */
+s32 dsp_audio_ulaw_to_s32[256];
+/* alaw[unsigned char] -> signed 16-bit */
+s32 dsp_audio_alaw_to_s32[256];
+
+s32 *dsp_audio_law_to_s32;
+EXPORT_SYMBOL(dsp_audio_law_to_s32);
+
+/* signed 16-bit -> law */
+u8 dsp_audio_s16_to_law[65536];
+EXPORT_SYMBOL(dsp_audio_s16_to_law);
+
+/* alaw -> ulaw */
+u8 dsp_audio_alaw_to_ulaw[256];
+/* ulaw -> alaw */
+u8 dsp_audio_ulaw_to_alaw[256];
+u8 dsp_silence;
+
+
+/*****************************************************
+ * generate table for conversion of s16 to alaw/ulaw *
+ *****************************************************/
+
+#define AMI_MASK 0x55
+
+static inline unsigned char linear2alaw(short int linear)
+{
+	int mask;
+	int seg;
+	int pcm_val;
+	static int seg_end[8] = {
+		0xFF, 0x1FF, 0x3FF, 0x7FF, 0xFFF, 0x1FFF, 0x3FFF, 0x7FFF
+	};
+
+	pcm_val = linear;
+	if (pcm_val >= 0) {
+		/* Sign (7th) bit = 1 */
+		mask = AMI_MASK | 0x80;
+	} else {
+		/* Sign bit = 0 */
+		mask = AMI_MASK;
+		pcm_val = -pcm_val;
+	}
+
+	/* Convert the scaled magnitude to segment number. */
+	for (seg = 0;  seg < 8;  seg++) {
+		if (pcm_val <= seg_end[seg])
+			break;
+	}
+	/* Combine the sign, segment, and quantization bits. */
+	return  ((seg << 4) |
+		 ((pcm_val >> ((seg)  ?  (seg + 3)  :  4)) & 0x0F)) ^ mask;
+}
+
+
+static inline short int alaw2linear(unsigned char alaw)
+{
+	int i;
+	int seg;
+
+	alaw ^= AMI_MASK;
+	i = ((alaw & 0x0F) << 4) + 8 /* rounding error */;
+	seg = (((int) alaw & 0x70) >> 4);
+	if (seg)
+		i = (i + 0x100) << (seg - 1);
+	return (short int) ((alaw & 0x80)  ?  i  :  -i);
+}
+
+static inline short int ulaw2linear(unsigned char ulaw)
+{
+	short mu, e, f, y;
+	static short etab[] = {0, 132, 396, 924, 1980, 4092, 8316, 16764};
+
+	mu = 255 - ulaw;
+	e = (mu & 0x70) / 16;
+	f = mu & 0x0f;
+	y = f * (1 << (e + 3));
+	y += etab[e];
+	if (mu & 0x80)
+		y = -y;
+	return y;
+}
+
+#define BIAS 0x84   /*!< define the add-in bias for 16 bit samples */
+
+static unsigned char linear2ulaw(short sample)
+{
+	static int exp_lut[256] = {
+		0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
+		4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,
+		5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+		5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+		6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+		7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
+	int sign, exponent, mantissa;
+	unsigned char ulawbyte;
+
+	/* Get the sample into sign-magnitude. */
+	sign = (sample >> 8) & 0x80;	  /* set aside the sign */
+	if (sign != 0)
+		sample = -sample;	      /* get magnitude */
+
+	/* Convert from 16 bit linear to ulaw. */
+	sample = sample + BIAS;
+	exponent = exp_lut[(sample >> 7) & 0xFF];
+	mantissa = (sample >> (exponent + 3)) & 0x0F;
+	ulawbyte = ~(sign | (exponent << 4) | mantissa);
+
+	return ulawbyte;
+}
+
+static int reverse_bits(int i)
+{
+	int z, j;
+	z = 0;
+
+	for (j = 0; j < 8; j++) {
+		if ((i & (1 << j)) != 0)
+			z |= 1 << (7 - j);
+	}
+	return z;
+}
+
+
+void dsp_audio_generate_law_tables(void)
+{
+	int i;
+	for (i = 0; i < 256; i++)
+		dsp_audio_alaw_to_s32[i] = alaw2linear(reverse_bits(i));
+
+	for (i = 0; i < 256; i++)
+		dsp_audio_ulaw_to_s32[i] = ulaw2linear(reverse_bits(i));
+
+	for (i = 0; i < 256; i++) {
+		dsp_audio_alaw_to_ulaw[i] =
+			linear2ulaw(dsp_audio_alaw_to_s32[i]);
+		dsp_audio_ulaw_to_alaw[i] =
+			linear2alaw(dsp_audio_ulaw_to_s32[i]);
+	}
+}
+
+void
+dsp_audio_generate_s2law_table(void)
+{
+	int i;
+
+	if (dsp_options & DSP_OPT_ULAW) {
+		/* generating ulaw-table */
+		for (i = -32768; i < 32768; i++) {
+			dsp_audio_s16_to_law[i & 0xffff] =
+				reverse_bits(linear2ulaw(i));
+		}
+	} else {
+		/* generating alaw-table */
+		for (i = -32768; i < 32768; i++) {
+			dsp_audio_s16_to_law[i & 0xffff] =
+				reverse_bits(linear2alaw(i));
+		}
+	}
+}
+
+
+/*
+ * the seven bit sample is the number of every second alaw-sample ordered by
+ * aplitude. 0x00 is negative, 0x7f is positive amplitude.
+ */
+u8 dsp_audio_seven2law[128];
+u8 dsp_audio_law2seven[256];
+
+/********************************************************************
+ * generate table for conversion law from/to 7-bit alaw-like sample *
+ ********************************************************************/
+
+void
+dsp_audio_generate_seven(void)
+{
+	int i, j, k;
+	u8 spl;
+	u8 sorted_alaw[256];
+
+	/* generate alaw table, sorted by the linear value */
+	for (i = 0; i < 256; i++) {
+		j = 0;
+		for (k = 0; k < 256; k++) {
+			if (dsp_audio_alaw_to_s32[k]
+				< dsp_audio_alaw_to_s32[i]) {
+			j++;
+			}
+		}
+		sorted_alaw[j] = i;
+	}
+
+	/* generate tabels */
+	for (i = 0; i < 256; i++) {
+		/* spl is the source: the law-sample (converted to alaw) */
+		spl = i;
+		if (dsp_options & DSP_OPT_ULAW)
+			spl = dsp_audio_ulaw_to_alaw[i];
+		/* find the 7-bit-sample */
+		for (j = 0; j < 256; j++) {
+			if (sorted_alaw[j] == spl)
+				break;
+		}
+		/* write 7-bit audio value */
+		dsp_audio_law2seven[i] = j >> 1;
+	}
+	for (i = 0; i < 128; i++) {
+		spl = sorted_alaw[i << 1];
+		if (dsp_options & DSP_OPT_ULAW)
+			spl = dsp_audio_alaw_to_ulaw[spl];
+		dsp_audio_seven2law[i] = spl;
+	}
+}
+
+
+/* mix 2*law -> law */
+u8 dsp_audio_mix_law[65536];
+
+/******************************************************
+ * generate mix table to mix two law samples into one *
+ ******************************************************/
+
+void
+dsp_audio_generate_mix_table(void)
+{
+	int i, j;
+	s32 sample;
+
+	i = 0;
+	while (i < 256) {
+		j = 0;
+		while (j < 256) {
+			sample = dsp_audio_law_to_s32[i];
+			sample += dsp_audio_law_to_s32[j];
+			if (sample > 32767)
+				sample = 32767;
+			if (sample < -32768)
+				sample = -32768;
+			dsp_audio_mix_law[(i<<8)|j] =
+				dsp_audio_s16_to_law[sample & 0xffff];
+			j++;
+		}
+		i++;
+	}
+}
+
+
+/*************************************
+ * generate different volume changes *
+ *************************************/
+
+static u8 dsp_audio_reduce8[256];
+static u8 dsp_audio_reduce7[256];
+static u8 dsp_audio_reduce6[256];
+static u8 dsp_audio_reduce5[256];
+static u8 dsp_audio_reduce4[256];
+static u8 dsp_audio_reduce3[256];
+static u8 dsp_audio_reduce2[256];
+static u8 dsp_audio_reduce1[256];
+static u8 dsp_audio_increase1[256];
+static u8 dsp_audio_increase2[256];
+static u8 dsp_audio_increase3[256];
+static u8 dsp_audio_increase4[256];
+static u8 dsp_audio_increase5[256];
+static u8 dsp_audio_increase6[256];
+static u8 dsp_audio_increase7[256];
+static u8 dsp_audio_increase8[256];
+
+static u8 *dsp_audio_volume_change[16] = {
+	dsp_audio_reduce8,
+	dsp_audio_reduce7,
+	dsp_audio_reduce6,
+	dsp_audio_reduce5,
+	dsp_audio_reduce4,
+	dsp_audio_reduce3,
+	dsp_audio_reduce2,
+	dsp_audio_reduce1,
+	dsp_audio_increase1,
+	dsp_audio_increase2,
+	dsp_audio_increase3,
+	dsp_audio_increase4,
+	dsp_audio_increase5,
+	dsp_audio_increase6,
+	dsp_audio_increase7,
+	dsp_audio_increase8,
+};
+
+void
+dsp_audio_generate_volume_changes(void)
+{
+	register s32 sample;
+	int i;
+	int num[]   = { 110, 125, 150, 175, 200, 300, 400, 500 };
+	int denum[] = { 100, 100, 100, 100, 100, 100, 100, 100 };
+
+	i = 0;
+	while (i < 256) {
+		dsp_audio_reduce8[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[7] / num[7]) & 0xffff];
+		dsp_audio_reduce7[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[6] / num[6]) & 0xffff];
+		dsp_audio_reduce6[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[5] / num[5]) & 0xffff];
+		dsp_audio_reduce5[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[4] / num[4]) & 0xffff];
+		dsp_audio_reduce4[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[3] / num[3]) & 0xffff];
+		dsp_audio_reduce3[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[2] / num[2]) & 0xffff];
+		dsp_audio_reduce2[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[1] / num[1]) & 0xffff];
+		dsp_audio_reduce1[i] = dsp_audio_s16_to_law[
+			(dsp_audio_law_to_s32[i] * denum[0] / num[0]) & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[0] / denum[0];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase1[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[1] / denum[1];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase2[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[2] / denum[2];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase3[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[3] / denum[3];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase4[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[4] / denum[4];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase5[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[5] / denum[5];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase6[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[6] / denum[6];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase7[i] = dsp_audio_s16_to_law[sample & 0xffff];
+		sample = dsp_audio_law_to_s32[i] * num[7] / denum[7];
+		if (sample < -32768)
+			sample = -32768;
+		else if (sample > 32767)
+			sample = 32767;
+		dsp_audio_increase8[i] = dsp_audio_s16_to_law[sample & 0xffff];
+
+		i++;
+	}
+}
+
+
+/**************************************
+ * change the volume of the given skb *
+ **************************************/
+
+/* this is a helper function for changing volume of skb. the range may be
+ * -8 to 8, which is a shift to the power of 2. 0 == no volume, 3 == volume*8
+ */
+void
+dsp_change_volume(struct sk_buff *skb, int volume)
+{
+	u8 *volume_change;
+	int i, ii;
+	u8 *p;
+	int shift;
+
+	if (volume == 0)
+		return;
+
+	/* get correct conversion table */
+	if (volume < 0) {
+		shift = volume + 8;
+		if (shift < 0)
+			shift = 0;
+	} else {
+		shift = volume + 7;
+		if (shift > 15)
+			shift = 15;
+	}
+	volume_change = dsp_audio_volume_change[shift];
+	i = 0;
+	ii = skb->len;
+	p = skb->data;
+	/* change volume */
+	while (i < ii) {
+		*p = volume_change[*p];
+		p++;
+		i++;
+	}
+}
+
diff --git a/drivers/isdn/mISDN/dsp_biquad.h b/drivers/isdn/mISDN/dsp_biquad.h
new file mode 100644
index 00000000000..038191bc45f
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_biquad.h
@@ -0,0 +1,65 @@
+/*
+ * SpanDSP - a series of DSP components for telephony
+ *
+ * biquad.h - General telephony bi-quad section routines (currently this just
+ *            handles canonic/type 2 form)
+ *
+ * Written by Steve Underwood <steveu@coppice.org>
+ *
+ * Copyright (C) 2001 Steve Underwood
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+struct biquad2_state {
+	int32_t gain;
+	int32_t a1;
+	int32_t a2;
+	int32_t b1;
+	int32_t b2;
+
+	int32_t z1;
+	int32_t z2;
+};
+
+static inline void biquad2_init(struct biquad2_state *bq,
+    int32_t gain, int32_t a1, int32_t a2, int32_t b1, int32_t b2)
+{
+	bq->gain = gain;
+	bq->a1 = a1;
+	bq->a2 = a2;
+	bq->b1 = b1;
+	bq->b2 = b2;
+
+	bq->z1 = 0;
+	bq->z2 = 0;
+}
+
+static inline int16_t biquad2(struct biquad2_state *bq, int16_t sample)
+{
+	int32_t y;
+	int32_t z0;
+
+	z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2;
+	y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2;
+
+	bq->z2 = bq->z1;
+	bq->z1 = z0 >> 15;
+	y >>= 15;
+	return  y;
+}
diff --git a/drivers/isdn/mISDN/dsp_blowfish.c b/drivers/isdn/mISDN/dsp_blowfish.c
new file mode 100644
index 00000000000..18e411e95bb
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_blowfish.c
@@ -0,0 +1,672 @@
+/*
+ * Blowfish encryption/decryption for mISDN_dsp.
+ *
+ * Copyright Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "core.h"
+#include "dsp.h"
+
+/*
+ * how to encode a sample stream to 64-bit blocks that will be encryped
+ *
+ * first of all, data is collected until a block of 9 samples are received.
+ * of course, a packet may have much more than 9 sample, but is may have
+ * not excacly the multiple of 9 samples. if there is a rest, the next
+ * received data will complete the block.
+ *
+ * the block is then converted to 9 uLAW samples without the least sigificant
+ * bit. the result is a 7-bit encoded sample.
+ *
+ * the samples will be reoganised to form 8 bytes of data:
+ * (5(6) means: encoded sample no. 5, bit 6)
+ *
+ * 0(6) 0(5) 0(4) 0(3) 0(2) 0(1) 0(0) 1(6)
+ * 1(5) 1(4) 1(3) 1(2) 1(1) 1(0) 2(6) 2(5)
+ * 2(4) 2(3) 2(2) 2(1) 2(0) 3(6) 3(5) 3(4)
+ * 3(3) 3(2) 3(1) 3(0) 4(6) 4(5) 4(4) 4(3)
+ * 4(2) 4(1) 4(0) 5(6) 5(5) 5(4) 5(3) 5(2)
+ * 5(1) 5(0) 6(6) 6(5) 6(4) 6(3) 6(2) 6(1)
+ * 6(0) 7(6) 7(5) 7(4) 7(3) 7(2) 7(1) 7(0)
+ * 8(6) 8(5) 8(4) 8(3) 8(2) 8(1) 8(0)
+ *
+ * the missing bit 0 of the last byte is filled with some
+ * random noise, to fill all 8 bytes.
+ *
+ * the 8 bytes will be encrypted using blowfish.
+ *
+ * the result will be converted into 9 bytes. the bit 7 is used for
+ * checksumme (CS) for sync (0, 1) and for the last bit:
+ * (5(6) means: crypted byte 5, bit 6)
+ *
+ * 1    0(7) 0(6) 0(5) 0(4) 0(3) 0(2) 0(1)
+ * 0    0(0) 1(7) 1(6) 1(5) 1(4) 1(3) 1(2)
+ * 0    1(1) 1(0) 2(7) 2(6) 2(5) 2(4) 2(3)
+ * 0    2(2) 2(1) 2(0) 3(7) 3(6) 3(5) 3(4)
+ * 0    3(3) 3(2) 3(1) 3(0) 4(7) 4(6) 4(5)
+ * CS   4(4) 4(3) 4(2) 4(1) 4(0) 5(7) 5(6)
+ * CS   5(5) 5(4) 5(3) 5(2) 5(1) 5(0) 6(7)
+ * CS   6(6) 6(5) 6(4) 6(3) 6(2) 6(1) 6(0)
+ * 7(0) 7(6) 7(5) 7(4) 7(3) 7(2) 7(1) 7(0)
+ *
+ * the checksum is used to detect transmission errors and frame drops.
+ *
+ * synchronisation of received block is done by shifting the upper bit of each
+ * byte (bit 7) to a shift register. if the rigister has the first five bits
+ * (10000), this is used to find the sync. only if sync has been found, the
+ * current block of 9 received bytes are decrypted. before that the check
+ * sum is calculated. if it is incorrect the block is dropped.
+ * this will avoid loud noise due to corrupt encrypted data.
+ *
+ * if the last block is corrupt, the current decoded block is repeated
+ * until a valid block has been received.
+ */
+
+/*
+ *  some blowfish parts are taken from the
+ * crypto-api for faster implementation
+ */
+
+struct bf_ctx {
+	u32 p[18];
+	u32 s[1024];
+};
+
+static const u32 bf_pbox[16 + 2] = {
+	0x243f6a88, 0x85a308d3, 0x13198a2e, 0x03707344,
+	0xa4093822, 0x299f31d0, 0x082efa98, 0xec4e6c89,
+	0x452821e6, 0x38d01377, 0xbe5466cf, 0x34e90c6c,
+	0xc0ac29b7, 0xc97c50dd, 0x3f84d5b5, 0xb5470917,
+	0x9216d5d9, 0x8979fb1b,
+};
+
+static const u32 bf_sbox[256 * 4] = {
+	0xd1310ba6, 0x98dfb5ac, 0x2ffd72db, 0xd01adfb7,
+	0xb8e1afed, 0x6a267e96, 0xba7c9045, 0xf12c7f99,
+	0x24a19947, 0xb3916cf7, 0x0801f2e2, 0x858efc16,
+	0x636920d8, 0x71574e69, 0xa458fea3, 0xf4933d7e,
+	0x0d95748f, 0x728eb658, 0x718bcd58, 0x82154aee,
+	0x7b54a41d, 0xc25a59b5, 0x9c30d539, 0x2af26013,
+	0xc5d1b023, 0x286085f0, 0xca417918, 0xb8db38ef,
+	0x8e79dcb0, 0x603a180e, 0x6c9e0e8b, 0xb01e8a3e,
+	0xd71577c1, 0xbd314b27, 0x78af2fda, 0x55605c60,
+	0xe65525f3, 0xaa55ab94, 0x57489862, 0x63e81440,
+	0x55ca396a, 0x2aab10b6, 0xb4cc5c34, 0x1141e8ce,
+	0xa15486af, 0x7c72e993, 0xb3ee1411, 0x636fbc2a,
+	0x2ba9c55d, 0x741831f6, 0xce5c3e16, 0x9b87931e,
+	0xafd6ba33, 0x6c24cf5c, 0x7a325381, 0x28958677,
+	0x3b8f4898, 0x6b4bb9af, 0xc4bfe81b, 0x66282193,
+	0x61d809cc, 0xfb21a991, 0x487cac60, 0x5dec8032,
+	0xef845d5d, 0xe98575b1, 0xdc262302, 0xeb651b88,
+	0x23893e81, 0xd396acc5, 0x0f6d6ff3, 0x83f44239,
+	0x2e0b4482, 0xa4842004, 0x69c8f04a, 0x9e1f9b5e,
+	0x21c66842, 0xf6e96c9a, 0x670c9c61, 0xabd388f0,
+	0x6a51a0d2, 0xd8542f68, 0x960fa728, 0xab5133a3,
+	0x6eef0b6c, 0x137a3be4, 0xba3bf050, 0x7efb2a98,
+	0xa1f1651d, 0x39af0176, 0x66ca593e, 0x82430e88,
+	0x8cee8619, 0x456f9fb4, 0x7d84a5c3, 0x3b8b5ebe,
+	0xe06f75d8, 0x85c12073, 0x401a449f, 0x56c16aa6,
+	0x4ed3aa62, 0x363f7706, 0x1bfedf72, 0x429b023d,
+	0x37d0d724, 0xd00a1248, 0xdb0fead3, 0x49f1c09b,
+	0x075372c9, 0x80991b7b, 0x25d479d8, 0xf6e8def7,
+	0xe3fe501a, 0xb6794c3b, 0x976ce0bd, 0x04c006ba,
+	0xc1a94fb6, 0x409f60c4, 0x5e5c9ec2, 0x196a2463,
+	0x68fb6faf, 0x3e6c53b5, 0x1339b2eb, 0x3b52ec6f,
+	0x6dfc511f, 0x9b30952c, 0xcc814544, 0xaf5ebd09,
+	0xbee3d004, 0xde334afd, 0x660f2807, 0x192e4bb3,
+	0xc0cba857, 0x45c8740f, 0xd20b5f39, 0xb9d3fbdb,
+	0x5579c0bd, 0x1a60320a, 0xd6a100c6, 0x402c7279,
+	0x679f25fe, 0xfb1fa3cc, 0x8ea5e9f8, 0xdb3222f8,
+	0x3c7516df, 0xfd616b15, 0x2f501ec8, 0xad0552ab,
+	0x323db5fa, 0xfd238760, 0x53317b48, 0x3e00df82,
+	0x9e5c57bb, 0xca6f8ca0, 0x1a87562e, 0xdf1769db,
+	0xd542a8f6, 0x287effc3, 0xac6732c6, 0x8c4f5573,
+	0x695b27b0, 0xbbca58c8, 0xe1ffa35d, 0xb8f011a0,
+	0x10fa3d98, 0xfd2183b8, 0x4afcb56c, 0x2dd1d35b,
+	0x9a53e479, 0xb6f84565, 0xd28e49bc, 0x4bfb9790,
+	0xe1ddf2da, 0xa4cb7e33, 0x62fb1341, 0xcee4c6e8,
+	0xef20cada, 0x36774c01, 0xd07e9efe, 0x2bf11fb4,
+	0x95dbda4d, 0xae909198, 0xeaad8e71, 0x6b93d5a0,
+	0xd08ed1d0, 0xafc725e0, 0x8e3c5b2f, 0x8e7594b7,
+	0x8ff6e2fb, 0xf2122b64, 0x8888b812, 0x900df01c,
+	0x4fad5ea0, 0x688fc31c, 0xd1cff191, 0xb3a8c1ad,
+	0x2f2f2218, 0xbe0e1777, 0xea752dfe, 0x8b021fa1,
+	0xe5a0cc0f, 0xb56f74e8, 0x18acf3d6, 0xce89e299,
+	0xb4a84fe0, 0xfd13e0b7, 0x7cc43b81, 0xd2ada8d9,
+	0x165fa266, 0x80957705, 0x93cc7314, 0x211a1477,
+	0xe6ad2065, 0x77b5fa86, 0xc75442f5, 0xfb9d35cf,
+	0xebcdaf0c, 0x7b3e89a0, 0xd6411bd3, 0xae1e7e49,
+	0x00250e2d, 0x2071b35e, 0x226800bb, 0x57b8e0af,
+	0x2464369b, 0xf009b91e, 0x5563911d, 0x59dfa6aa,
+	0x78c14389, 0xd95a537f, 0x207d5ba2, 0x02e5b9c5,
+	0x83260376, 0x6295cfa9, 0x11c81968, 0x4e734a41,
+	0xb3472dca, 0x7b14a94a, 0x1b510052, 0x9a532915,
+	0xd60f573f, 0xbc9bc6e4, 0x2b60a476, 0x81e67400,
+	0x08ba6fb5, 0x571be91f, 0xf296ec6b, 0x2a0dd915,
+	0xb6636521, 0xe7b9f9b6, 0xff34052e, 0xc5855664,
+	0x53b02d5d, 0xa99f8fa1, 0x08ba4799, 0x6e85076a,
+	0x4b7a70e9, 0xb5b32944, 0xdb75092e, 0xc4192623,
+	0xad6ea6b0, 0x49a7df7d, 0x9cee60b8, 0x8fedb266,
+	0xecaa8c71, 0x699a17ff, 0x5664526c, 0xc2b19ee1,
+	0x193602a5, 0x75094c29, 0xa0591340, 0xe4183a3e,
+	0x3f54989a, 0x5b429d65, 0x6b8fe4d6, 0x99f73fd6,
+	0xa1d29c07, 0xefe830f5, 0x4d2d38e6, 0xf0255dc1,
+	0x4cdd2086, 0x8470eb26, 0x6382e9c6, 0x021ecc5e,
+	0x09686b3f, 0x3ebaefc9, 0x3c971814, 0x6b6a70a1,
+	0x687f3584, 0x52a0e286, 0xb79c5305, 0xaa500737,
+	0x3e07841c, 0x7fdeae5c, 0x8e7d44ec, 0x5716f2b8,
+	0xb03ada37, 0xf0500c0d, 0xf01c1f04, 0x0200b3ff,
+	0xae0cf51a, 0x3cb574b2, 0x25837a58, 0xdc0921bd,
+	0xd19113f9, 0x7ca92ff6, 0x94324773, 0x22f54701,
+	0x3ae5e581, 0x37c2dadc, 0xc8b57634, 0x9af3dda7,
+	0xa9446146, 0x0fd0030e, 0xecc8c73e, 0xa4751e41,
+	0xe238cd99, 0x3bea0e2f, 0x3280bba1, 0x183eb331,
+	0x4e548b38, 0x4f6db908, 0x6f420d03, 0xf60a04bf,
+	0x2cb81290, 0x24977c79, 0x5679b072, 0xbcaf89af,
+	0xde9a771f, 0xd9930810, 0xb38bae12, 0xdccf3f2e,
+	0x5512721f, 0x2e6b7124, 0x501adde6, 0x9f84cd87,
+	0x7a584718, 0x7408da17, 0xbc9f9abc, 0xe94b7d8c,
+	0xec7aec3a, 0xdb851dfa, 0x63094366, 0xc464c3d2,
+	0xef1c1847, 0x3215d908, 0xdd433b37, 0x24c2ba16,
+	0x12a14d43, 0x2a65c451, 0x50940002, 0x133ae4dd,
+	0x71dff89e, 0x10314e55, 0x81ac77d6, 0x5f11199b,
+	0x043556f1, 0xd7a3c76b, 0x3c11183b, 0x5924a509,
+	0xf28fe6ed, 0x97f1fbfa, 0x9ebabf2c, 0x1e153c6e,
+	0x86e34570, 0xeae96fb1, 0x860e5e0a, 0x5a3e2ab3,
+	0x771fe71c, 0x4e3d06fa, 0x2965dcb9, 0x99e71d0f,
+	0x803e89d6, 0x5266c825, 0x2e4cc978, 0x9c10b36a,
+	0xc6150eba, 0x94e2ea78, 0xa5fc3c53, 0x1e0a2df4,
+	0xf2f74ea7, 0x361d2b3d, 0x1939260f, 0x19c27960,
+	0x5223a708, 0xf71312b6, 0xebadfe6e, 0xeac31f66,
+	0xe3bc4595, 0xa67bc883, 0xb17f37d1, 0x018cff28,
+	0xc332ddef, 0xbe6c5aa5, 0x65582185, 0x68ab9802,
+	0xeecea50f, 0xdb2f953b, 0x2aef7dad, 0x5b6e2f84,
+	0x1521b628, 0x29076170, 0xecdd4775, 0x619f1510,
+	0x13cca830, 0xeb61bd96, 0x0334fe1e, 0xaa0363cf,
+	0xb5735c90, 0x4c70a239, 0xd59e9e0b, 0xcbaade14,
+	0xeecc86bc, 0x60622ca7, 0x9cab5cab, 0xb2f3846e,
+	0x648b1eaf, 0x19bdf0ca, 0xa02369b9, 0x655abb50,
+	0x40685a32, 0x3c2ab4b3, 0x319ee9d5, 0xc021b8f7,
+	0x9b540b19, 0x875fa099, 0x95f7997e, 0x623d7da8,
+	0xf837889a, 0x97e32d77, 0x11ed935f, 0x16681281,
+	0x0e358829, 0xc7e61fd6, 0x96dedfa1, 0x7858ba99,
+	0x57f584a5, 0x1b227263, 0x9b83c3ff, 0x1ac24696,
+	0xcdb30aeb, 0x532e3054, 0x8fd948e4, 0x6dbc3128,
+	0x58ebf2ef, 0x34c6ffea, 0xfe28ed61, 0xee7c3c73,
+	0x5d4a14d9, 0xe864b7e3, 0x42105d14, 0x203e13e0,
+	0x45eee2b6, 0xa3aaabea, 0xdb6c4f15, 0xfacb4fd0,
+	0xc742f442, 0xef6abbb5, 0x654f3b1d, 0x41cd2105,
+	0xd81e799e, 0x86854dc7, 0xe44b476a, 0x3d816250,
+	0xcf62a1f2, 0x5b8d2646, 0xfc8883a0, 0xc1c7b6a3,
+	0x7f1524c3, 0x69cb7492, 0x47848a0b, 0x5692b285,
+	0x095bbf00, 0xad19489d, 0x1462b174, 0x23820e00,
+	0x58428d2a, 0x0c55f5ea, 0x1dadf43e, 0x233f7061,
+	0x3372f092, 0x8d937e41, 0xd65fecf1, 0x6c223bdb,
+	0x7cde3759, 0xcbee7460, 0x4085f2a7, 0xce77326e,
+	0xa6078084, 0x19f8509e, 0xe8efd855, 0x61d99735,
+	0xa969a7aa, 0xc50c06c2, 0x5a04abfc, 0x800bcadc,
+	0x9e447a2e, 0xc3453484, 0xfdd56705, 0x0e1e9ec9,
+	0xdb73dbd3, 0x105588cd, 0x675fda79, 0xe3674340,
+	0xc5c43465, 0x713e38d8, 0x3d28f89e, 0xf16dff20,
+	0x153e21e7, 0x8fb03d4a, 0xe6e39f2b, 0xdb83adf7,
+	0xe93d5a68, 0x948140f7, 0xf64c261c, 0x94692934,
+	0x411520f7, 0x7602d4f7, 0xbcf46b2e, 0xd4a20068,
+	0xd4082471, 0x3320f46a, 0x43b7d4b7, 0x500061af,
+	0x1e39f62e, 0x97244546, 0x14214f74, 0xbf8b8840,
+	0x4d95fc1d, 0x96b591af, 0x70f4ddd3, 0x66a02f45,
+	0xbfbc09ec, 0x03bd9785, 0x7fac6dd0, 0x31cb8504,
+	0x96eb27b3, 0x55fd3941, 0xda2547e6, 0xabca0a9a,
+	0x28507825, 0x530429f4, 0x0a2c86da, 0xe9b66dfb,
+	0x68dc1462, 0xd7486900, 0x680ec0a4, 0x27a18dee,
+	0x4f3ffea2, 0xe887ad8c, 0xb58ce006, 0x7af4d6b6,
+	0xaace1e7c, 0xd3375fec, 0xce78a399, 0x406b2a42,
+	0x20fe9e35, 0xd9f385b9, 0xee39d7ab, 0x3b124e8b,
+	0x1dc9faf7, 0x4b6d1856, 0x26a36631, 0xeae397b2,
+	0x3a6efa74, 0xdd5b4332, 0x6841e7f7, 0xca7820fb,
+	0xfb0af54e, 0xd8feb397, 0x454056ac, 0xba489527,
+	0x55533a3a, 0x20838d87, 0xfe6ba9b7, 0xd096954b,
+	0x55a867bc, 0xa1159a58, 0xcca92963, 0x99e1db33,
+	0xa62a4a56, 0x3f3125f9, 0x5ef47e1c, 0x9029317c,
+	0xfdf8e802, 0x04272f70, 0x80bb155c, 0x05282ce3,
+	0x95c11548, 0xe4c66d22, 0x48c1133f, 0xc70f86dc,
+	0x07f9c9ee, 0x41041f0f, 0x404779a4, 0x5d886e17,
+	0x325f51eb, 0xd59bc0d1, 0xf2bcc18f, 0x41113564,
+	0x257b7834, 0x602a9c60, 0xdff8e8a3, 0x1f636c1b,
+	0x0e12b4c2, 0x02e1329e, 0xaf664fd1, 0xcad18115,
+	0x6b2395e0, 0x333e92e1, 0x3b240b62, 0xeebeb922,
+	0x85b2a20e, 0xe6ba0d99, 0xde720c8c, 0x2da2f728,
+	0xd0127845, 0x95b794fd, 0x647d0862, 0xe7ccf5f0,
+	0x5449a36f, 0x877d48fa, 0xc39dfd27, 0xf33e8d1e,
+	0x0a476341, 0x992eff74, 0x3a6f6eab, 0xf4f8fd37,
+	0xa812dc60, 0xa1ebddf8, 0x991be14c, 0xdb6e6b0d,
+	0xc67b5510, 0x6d672c37, 0x2765d43b, 0xdcd0e804,
+	0xf1290dc7, 0xcc00ffa3, 0xb5390f92, 0x690fed0b,
+	0x667b9ffb, 0xcedb7d9c, 0xa091cf0b, 0xd9155ea3,
+	0xbb132f88, 0x515bad24, 0x7b9479bf, 0x763bd6eb,
+	0x37392eb3, 0xcc115979, 0x8026e297, 0xf42e312d,
+	0x6842ada7, 0xc66a2b3b, 0x12754ccc, 0x782ef11c,
+	0x6a124237, 0xb79251e7, 0x06a1bbe6, 0x4bfb6350,
+	0x1a6b1018, 0x11caedfa, 0x3d25bdd8, 0xe2e1c3c9,
+	0x44421659, 0x0a121386, 0xd90cec6e, 0xd5abea2a,
+	0x64af674e, 0xda86a85f, 0xbebfe988, 0x64e4c3fe,
+	0x9dbc8057, 0xf0f7c086, 0x60787bf8, 0x6003604d,
+	0xd1fd8346, 0xf6381fb0, 0x7745ae04, 0xd736fccc,
+	0x83426b33, 0xf01eab71, 0xb0804187, 0x3c005e5f,
+	0x77a057be, 0xbde8ae24, 0x55464299, 0xbf582e61,
+	0x4e58f48f, 0xf2ddfda2, 0xf474ef38, 0x8789bdc2,
+	0x5366f9c3, 0xc8b38e74, 0xb475f255, 0x46fcd9b9,
+	0x7aeb2661, 0x8b1ddf84, 0x846a0e79, 0x915f95e2,
+	0x466e598e, 0x20b45770, 0x8cd55591, 0xc902de4c,
+	0xb90bace1, 0xbb8205d0, 0x11a86248, 0x7574a99e,
+	0xb77f19b6, 0xe0a9dc09, 0x662d09a1, 0xc4324633,
+	0xe85a1f02, 0x09f0be8c, 0x4a99a025, 0x1d6efe10,
+	0x1ab93d1d, 0x0ba5a4df, 0xa186f20f, 0x2868f169,
+	0xdcb7da83, 0x573906fe, 0xa1e2ce9b, 0x4fcd7f52,
+	0x50115e01, 0xa70683fa, 0xa002b5c4, 0x0de6d027,
+	0x9af88c27, 0x773f8641, 0xc3604c06, 0x61a806b5,
+	0xf0177a28, 0xc0f586e0, 0x006058aa, 0x30dc7d62,
+	0x11e69ed7, 0x2338ea63, 0x53c2dd94, 0xc2c21634,
+	0xbbcbee56, 0x90bcb6de, 0xebfc7da1, 0xce591d76,
+	0x6f05e409, 0x4b7c0188, 0x39720a3d, 0x7c927c24,
+	0x86e3725f, 0x724d9db9, 0x1ac15bb4, 0xd39eb8fc,
+	0xed545578, 0x08fca5b5, 0xd83d7cd3, 0x4dad0fc4,
+	0x1e50ef5e, 0xb161e6f8, 0xa28514d9, 0x6c51133c,
+	0x6fd5c7e7, 0x56e14ec4, 0x362abfce, 0xddc6c837,
+	0xd79a3234, 0x92638212, 0x670efa8e, 0x406000e0,
+	0x3a39ce37, 0xd3faf5cf, 0xabc27737, 0x5ac52d1b,
+	0x5cb0679e, 0x4fa33742, 0xd3822740, 0x99bc9bbe,
+	0xd5118e9d, 0xbf0f7315, 0xd62d1c7e, 0xc700c47b,
+	0xb78c1b6b, 0x21a19045, 0xb26eb1be, 0x6a366eb4,
+	0x5748ab2f, 0xbc946e79, 0xc6a376d2, 0x6549c2c8,
+	0x530ff8ee, 0x468dde7d, 0xd5730a1d, 0x4cd04dc6,
+	0x2939bbdb, 0xa9ba4650, 0xac9526e8, 0xbe5ee304,
+	0xa1fad5f0, 0x6a2d519a, 0x63ef8ce2, 0x9a86ee22,
+	0xc089c2b8, 0x43242ef6, 0xa51e03aa, 0x9cf2d0a4,
+	0x83c061ba, 0x9be96a4d, 0x8fe51550, 0xba645bd6,
+	0x2826a2f9, 0xa73a3ae1, 0x4ba99586, 0xef5562e9,
+	0xc72fefd3, 0xf752f7da, 0x3f046f69, 0x77fa0a59,
+	0x80e4a915, 0x87b08601, 0x9b09e6ad, 0x3b3ee593,
+	0xe990fd5a, 0x9e34d797, 0x2cf0b7d9, 0x022b8b51,
+	0x96d5ac3a, 0x017da67d, 0xd1cf3ed6, 0x7c7d2d28,
+	0x1f9f25cf, 0xadf2b89b, 0x5ad6b472, 0x5a88f54c,
+	0xe029ac71, 0xe019a5e6, 0x47b0acfd, 0xed93fa9b,
+	0xe8d3c48d, 0x283b57cc, 0xf8d56629, 0x79132e28,
+	0x785f0191, 0xed756055, 0xf7960e44, 0xe3d35e8c,
+	0x15056dd4, 0x88f46dba, 0x03a16125, 0x0564f0bd,
+	0xc3eb9e15, 0x3c9057a2, 0x97271aec, 0xa93a072a,
+	0x1b3f6d9b, 0x1e6321f5, 0xf59c66fb, 0x26dcf319,
+	0x7533d928, 0xb155fdf5, 0x03563482, 0x8aba3cbb,
+	0x28517711, 0xc20ad9f8, 0xabcc5167, 0xccad925f,
+	0x4de81751, 0x3830dc8e, 0x379d5862, 0x9320f991,
+	0xea7a90c2, 0xfb3e7bce, 0x5121ce64, 0x774fbe32,
+	0xa8b6e37e, 0xc3293d46, 0x48de5369, 0x6413e680,
+	0xa2ae0810, 0xdd6db224, 0x69852dfd, 0x09072166,
+	0xb39a460a, 0x6445c0dd, 0x586cdecf, 0x1c20c8ae,
+	0x5bbef7dd, 0x1b588d40, 0xccd2017f, 0x6bb4e3bb,
+	0xdda26a7e, 0x3a59ff45, 0x3e350a44, 0xbcb4cdd5,
+	0x72eacea8, 0xfa6484bb, 0x8d6612ae, 0xbf3c6f47,
+	0xd29be463, 0x542f5d9e, 0xaec2771b, 0xf64e6370,
+	0x740e0d8d, 0xe75b1357, 0xf8721671, 0xaf537d5d,
+	0x4040cb08, 0x4eb4e2cc, 0x34d2466a, 0x0115af84,
+	0xe1b00428, 0x95983a1d, 0x06b89fb4, 0xce6ea048,
+	0x6f3f3b82, 0x3520ab82, 0x011a1d4b, 0x277227f8,
+	0x611560b1, 0xe7933fdc, 0xbb3a792b, 0x344525bd,
+	0xa08839e1, 0x51ce794b, 0x2f32c9b7, 0xa01fbac9,
+	0xe01cc87e, 0xbcc7d1f6, 0xcf0111c3, 0xa1e8aac7,
+	0x1a908749, 0xd44fbd9a, 0xd0dadecb, 0xd50ada38,
+	0x0339c32a, 0xc6913667, 0x8df9317c, 0xe0b12b4f,
+	0xf79e59b7, 0x43f5bb3a, 0xf2d519ff, 0x27d9459c,
+	0xbf97222c, 0x15e6fc2a, 0x0f91fc71, 0x9b941525,
+	0xfae59361, 0xceb69ceb, 0xc2a86459, 0x12baa8d1,
+	0xb6c1075e, 0xe3056a0c, 0x10d25065, 0xcb03a442,
+	0xe0ec6e0e, 0x1698db3b, 0x4c98a0be, 0x3278e964,
+	0x9f1f9532, 0xe0d392df, 0xd3a0342b, 0x8971f21e,
+	0x1b0a7441, 0x4ba3348c, 0xc5be7120, 0xc37632d8,
+	0xdf359f8d, 0x9b992f2e, 0xe60b6f47, 0x0fe3f11d,
+	0xe54cda54, 0x1edad891, 0xce6279cf, 0xcd3e7e6f,
+	0x1618b166, 0xfd2c1d05, 0x848fd2c5, 0xf6fb2299,
+	0xf523f357, 0xa6327623, 0x93a83531, 0x56cccd02,
+	0xacf08162, 0x5a75ebb5, 0x6e163697, 0x88d273cc,
+	0xde966292, 0x81b949d0, 0x4c50901b, 0x71c65614,
+	0xe6c6c7bd, 0x327a140a, 0x45e1d006, 0xc3f27b9a,
+	0xc9aa53fd, 0x62a80f00, 0xbb25bfe2, 0x35bdd2f6,
+	0x71126905, 0xb2040222, 0xb6cbcf7c, 0xcd769c2b,
+	0x53113ec0, 0x1640e3d3, 0x38abbd60, 0x2547adf0,
+	0xba38209c, 0xf746ce76, 0x77afa1c5, 0x20756060,
+	0x85cbfe4e, 0x8ae88dd8, 0x7aaaf9b0, 0x4cf9aa7e,
+	0x1948c25c, 0x02fb8a8c, 0x01c36ae4, 0xd6ebe1f9,
+	0x90d4f869, 0xa65cdea0, 0x3f09252d, 0xc208e69f,
+	0xb74e6132, 0xce77e25b, 0x578fdfe3, 0x3ac372e6,
+};
+
+/*
+ * Round loop unrolling macros, S is a pointer to a S-Box array
+ * organized in 4 unsigned longs at a row.
+ */
+#define GET32_3(x) (((x) & 0xff))
+#define GET32_2(x) (((x) >> (8)) & (0xff))
+#define GET32_1(x) (((x) >> (16)) & (0xff))
+#define GET32_0(x) (((x) >> (24)) & (0xff))
+
+#define bf_F(x) (((S[GET32_0(x)] + S[256 + GET32_1(x)]) ^ \
+    S[512 + GET32_2(x)]) + S[768 + GET32_3(x)])
+
+#define EROUND(a, b, n)  do { b ^= P[n]; a ^= bf_F(b); } while (0)
+#define DROUND(a, b, n)  do { a ^= bf_F(b); b ^= P[n]; } while (0)
+
+
+/*
+ * encrypt isdn data frame
+ * every block with 9 samples is encrypted
+ */
+void
+dsp_bf_encrypt(struct dsp *dsp, u8 *data, int len)
+{
+	int i = 0, j = dsp->bf_crypt_pos;
+	u8 *bf_data_in = dsp->bf_data_in;
+	u8 *bf_crypt_out = dsp->bf_crypt_out;
+	u32 *P = dsp->bf_p;
+	u32 *S = dsp->bf_s;
+	u32 yl, yr;
+	u32 cs;
+	u8 nibble;
+
+	while (i < len) {
+		/* collect a block of 9 samples */
+		if (j < 9) {
+			bf_data_in[j] = *data;
+			*data++ = bf_crypt_out[j++];
+			i++;
+			continue;
+		}
+		j = 0;
+		/* transcode 9 samples xlaw to 8 bytes */
+		yl = dsp_audio_law2seven[bf_data_in[0]];
+		yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[1]];
+		yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[2]];
+		yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[3]];
+		nibble = dsp_audio_law2seven[bf_data_in[4]];
+		yr = nibble;
+		yl = (yl<<4) | (nibble>>3);
+		yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[5]];
+		yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[6]];
+		yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[7]];
+		yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[8]];
+		yr = (yr<<1) | (bf_data_in[0] & 1);
+
+		/* fill unused bit with random noise of audio input */
+		/* encrypt */
+
+		EROUND(yr, yl, 0);
+		EROUND(yl, yr, 1);
+		EROUND(yr, yl, 2);
+		EROUND(yl, yr, 3);
+		EROUND(yr, yl, 4);
+		EROUND(yl, yr, 5);
+		EROUND(yr, yl, 6);
+		EROUND(yl, yr, 7);
+		EROUND(yr, yl, 8);
+		EROUND(yl, yr, 9);
+		EROUND(yr, yl, 10);
+		EROUND(yl, yr, 11);
+		EROUND(yr, yl, 12);
+		EROUND(yl, yr, 13);
+		EROUND(yr, yl, 14);
+		EROUND(yl, yr, 15);
+		yl ^= P[16];
+		yr ^= P[17];
+
+		/* calculate 3-bit checksumme */
+		cs = yl ^ (yl>>3) ^ (yl>>6) ^ (yl>>9) ^ (yl>>12) ^ (yl>>15)
+			^ (yl>>18) ^ (yl>>21) ^ (yl>>24) ^ (yl>>27) ^ (yl>>30)
+			^ (yr<<2) ^ (yr>>1) ^ (yr>>4) ^ (yr>>7) ^ (yr>>10)
+			^ (yr>>13) ^ (yr>>16) ^ (yr>>19) ^ (yr>>22) ^ (yr>>25)
+			^ (yr>>28) ^ (yr>>31);
+
+		/*
+		 * transcode 8 crypted bytes to 9 data bytes with sync
+		 * and checksum information
+		 */
+		bf_crypt_out[0] = (yl>>25) | 0x80;
+		bf_crypt_out[1] = (yl>>18) & 0x7f;
+		bf_crypt_out[2] = (yl>>11) & 0x7f;
+		bf_crypt_out[3] = (yl>>4) & 0x7f;
+		bf_crypt_out[4] = ((yl<<3) & 0x78) | ((yr>>29) & 0x07);
+		bf_crypt_out[5] = ((yr>>22) & 0x7f) | ((cs<<5) & 0x80);
+		bf_crypt_out[6] = ((yr>>15) & 0x7f) | ((cs<<6) & 0x80);
+		bf_crypt_out[7] = ((yr>>8) & 0x7f) | (cs<<7);
+		bf_crypt_out[8] = yr;
+	}
+
+	/* write current count */
+	dsp->bf_crypt_pos = j;
+
+}
+
+
+/*
+ * decrypt isdn data frame
+ * every block with 9 bytes is decrypted
+ */
+void
+dsp_bf_decrypt(struct dsp *dsp, u8 *data, int len)
+{
+	int i = 0;
+	u8 j = dsp->bf_decrypt_in_pos;
+	u8 k = dsp->bf_decrypt_out_pos;
+	u8 *bf_crypt_inring = dsp->bf_crypt_inring;
+	u8 *bf_data_out = dsp->bf_data_out;
+	u16 sync = dsp->bf_sync;
+	u32 *P = dsp->bf_p;
+	u32 *S = dsp->bf_s;
+	u32 yl, yr;
+	u8 nibble;
+	u8 cs, cs0, cs1, cs2;
+
+	while (i < len) {
+		/*
+		 * shift upper bit and rotate data to buffer ring
+		 * send current decrypted data
+		 */
+		sync = (sync<<1) | ((*data)>>7);
+		bf_crypt_inring[j++ & 15] = *data;
+		*data++ = bf_data_out[k++];
+		i++;
+		if (k == 9)
+			k = 0; /* repeat if no sync has been found */
+		/* check if not in sync */
+		if ((sync&0x1f0) != 0x100)
+			continue;
+		j -= 9;
+		/* transcode receive data to 64 bit block of encrypted data */
+		yl = bf_crypt_inring[j++ & 15];
+		yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */
+		yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */
+		yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */
+		nibble = bf_crypt_inring[j++ & 15]; /* bit7 = 0 */
+		yr = nibble;
+		yl = (yl<<4) | (nibble>>3);
+		cs2 = bf_crypt_inring[j++ & 15];
+		yr = (yr<<7) | (cs2 & 0x7f);
+		cs1 = bf_crypt_inring[j++ & 15];
+		yr = (yr<<7) | (cs1 & 0x7f);
+		cs0 = bf_crypt_inring[j++ & 15];
+		yr = (yr<<7) | (cs0 & 0x7f);
+		yr = (yr<<8) | bf_crypt_inring[j++ & 15];
+
+		/* calculate 3-bit checksumme */
+		cs = yl ^ (yl>>3) ^ (yl>>6) ^ (yl>>9) ^ (yl>>12) ^ (yl>>15)
+			^ (yl>>18) ^ (yl>>21) ^ (yl>>24) ^ (yl>>27) ^ (yl>>30)
+			^ (yr<<2) ^ (yr>>1) ^ (yr>>4) ^ (yr>>7) ^ (yr>>10)
+			^ (yr>>13) ^ (yr>>16) ^ (yr>>19) ^ (yr>>22) ^ (yr>>25)
+			^ (yr>>28) ^ (yr>>31);
+
+		/* check if frame is valid */
+		if ((cs&0x7) != (((cs2>>5)&4) | ((cs1>>6)&2) | (cs0 >> 7))) {
+			if (dsp_debug & DEBUG_DSP_BLOWFISH)
+				printk(KERN_DEBUG
+				    "DSP BLOWFISH: received corrupt frame, "
+				    "checksumme is not correct\n");
+			continue;
+		}
+
+		/* decrypt */
+		yr ^= P[17];
+		yl ^= P[16];
+		DROUND(yl, yr, 15);
+		DROUND(yr, yl, 14);
+		DROUND(yl, yr, 13);
+		DROUND(yr, yl, 12);
+		DROUND(yl, yr, 11);
+		DROUND(yr, yl, 10);
+		DROUND(yl, yr, 9);
+		DROUND(yr, yl, 8);
+		DROUND(yl, yr, 7);
+		DROUND(yr, yl, 6);
+		DROUND(yl, yr, 5);
+		DROUND(yr, yl, 4);
+		DROUND(yl, yr, 3);
+		DROUND(yr, yl, 2);
+		DROUND(yl, yr, 1);
+		DROUND(yr, yl, 0);
+
+		/* transcode 8 crypted bytes to 9 sample bytes */
+		bf_data_out[0] = dsp_audio_seven2law[(yl>>25) & 0x7f];
+		bf_data_out[1] = dsp_audio_seven2law[(yl>>18) & 0x7f];
+		bf_data_out[2] = dsp_audio_seven2law[(yl>>11) & 0x7f];
+		bf_data_out[3] = dsp_audio_seven2law[(yl>>4) & 0x7f];
+		bf_data_out[4] = dsp_audio_seven2law[((yl<<3) & 0x78) |
+		    ((yr>>29) & 0x07)];
+
+		bf_data_out[5] = dsp_audio_seven2law[(yr>>22) & 0x7f];
+		bf_data_out[6] = dsp_audio_seven2law[(yr>>15) & 0x7f];
+		bf_data_out[7] = dsp_audio_seven2law[(yr>>8) & 0x7f];
+		bf_data_out[8] = dsp_audio_seven2law[(yr>>1) & 0x7f];
+		k = 0; /* start with new decoded frame */
+	}
+
+	/* write current count and sync */
+	dsp->bf_decrypt_in_pos = j;
+	dsp->bf_decrypt_out_pos = k;
+	dsp->bf_sync = sync;
+}
+
+
+/* used to encrypt S and P boxes */
+static inline void
+encrypt_block(const u32 *P, const u32 *S, u32 *dst, u32 *src)
+{
+	u32 yl = src[0];
+	u32 yr = src[1];
+
+	EROUND(yr, yl, 0);
+	EROUND(yl, yr, 1);
+	EROUND(yr, yl, 2);
+	EROUND(yl, yr, 3);
+	EROUND(yr, yl, 4);
+	EROUND(yl, yr, 5);
+	EROUND(yr, yl, 6);
+	EROUND(yl, yr, 7);
+	EROUND(yr, yl, 8);
+	EROUND(yl, yr, 9);
+	EROUND(yr, yl, 10);
+	EROUND(yl, yr, 11);
+	EROUND(yr, yl, 12);
+	EROUND(yl, yr, 13);
+	EROUND(yr, yl, 14);
+	EROUND(yl, yr, 15);
+
+	yl ^= P[16];
+	yr ^= P[17];
+
+	dst[0] = yr;
+	dst[1] = yl;
+}
+
+/*
+ * initialize the dsp for encryption and decryption using the same key
+ * Calculates the blowfish S and P boxes for encryption and decryption.
+ * The margin of keylen must be 4-56 bytes.
+ * returns 0 if ok.
+ */
+int
+dsp_bf_init(struct dsp *dsp, const u8 *key, uint keylen)
+{
+	short i, j, count;
+	u32 data[2], temp;
+	u32 *P = (u32 *)dsp->bf_p;
+	u32 *S = (u32 *)dsp->bf_s;
+
+	if (keylen < 4 || keylen > 56)
+		return 1;
+
+	/* Set dsp states */
+	i = 0;
+	while (i < 9) {
+		dsp->bf_crypt_out[i] = 0xff;
+		dsp->bf_data_out[i] = dsp_silence;
+		i++;
+	}
+	dsp->bf_crypt_pos = 0;
+	dsp->bf_decrypt_in_pos = 0;
+	dsp->bf_decrypt_out_pos = 0;
+	dsp->bf_sync = 0x1ff;
+	dsp->bf_enable = 1;
+
+	/* Copy the initialization s-boxes */
+	for (i = 0, count = 0; i < 256; i++)
+		for (j = 0; j < 4; j++, count++)
+			S[count] = bf_sbox[count];
+
+	/* Set the p-boxes */
+	for (i = 0; i < 16 + 2; i++)
+		P[i] = bf_pbox[i];
+
+	/* Actual subkey generation */
+	for (j = 0, i = 0; i < 16 + 2; i++) {
+		temp = (((u32)key[j] << 24) |
+		    ((u32)key[(j + 1) % keylen] << 16) |
+		    ((u32)key[(j + 2) % keylen] << 8) |
+		    ((u32)key[(j + 3) % keylen]));
+
+		P[i] = P[i] ^ temp;
+		j = (j + 4) % keylen;
+	}
+
+	data[0] = 0x00000000;
+	data[1] = 0x00000000;
+
+	for (i = 0; i < 16 + 2; i += 2) {
+		encrypt_block(P, S, data, data);
+
+		P[i] = data[0];
+		P[i + 1] = data[1];
+	}
+
+	for (i = 0; i < 4; i++) {
+		for (j = 0, count = i * 256; j < 256; j += 2, count += 2) {
+			encrypt_block(P, S, data, data);
+
+			S[count] = data[0];
+			S[count + 1] = data[1];
+		}
+	}
+
+	return 0;
+}
+
+
+/*
+ * turn encryption off
+ */
+void
+dsp_bf_cleanup(struct dsp *dsp)
+{
+	dsp->bf_enable = 0;
+}
diff --git a/drivers/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c
new file mode 100644
index 00000000000..e92b1ba4b45
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_cmx.c
@@ -0,0 +1,1886 @@
+/*
+ * Audio crossconnecting/conferrencing (hardware level).
+ *
+ * Copyright 2002 by Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+/*
+ * The process of adding and removing parties to/from a conference:
+ *
+ * There is a chain of struct dsp_conf which has one or more members in a chain
+ * of struct dsp_conf_member.
+ *
+ * After a party is added, the conference is checked for hardware capability.
+ * Also if a party is removed, the conference is checked again.
+ *
+ * There are 3 different solutions: -1 = software, 0 = hardware-crossconnect
+ * 1-n = hardware-conference. The n will give the conference number.
+ *
+ * Depending on the change after removal or insertion of a party, hardware
+ * commands are given.
+ *
+ * The current solution is stored within the struct dsp_conf entry.
+ */
+
+/*
+ * HOW THE CMX WORKS:
+ *
+ * There are 3 types of interaction: One member is alone, in this case only
+ * data flow from upper to lower layer is done.
+ * Two members will also exchange their data so they are crossconnected.
+ * Three or more members will be added in a conference and will hear each
+ * other but will not receive their own speech (echo) if not enabled.
+ *
+ * Features of CMX are:
+ *  - Crossconnecting or even conference, if more than two members are together.
+ *  - Force mixing of transmit data with other crossconnect/conference members.
+ *  - Echo generation to benchmark the delay of audio processing.
+ *  - Use hardware to minimize cpu load, disable FIFO load and minimize delay.
+ *  - Dejittering and clock generation.
+ *
+ * There are 2 buffers:
+ *
+ *
+ * RX-Buffer
+ *                 R             W
+ *                 |             |
+ * ----------------+-------------+-------------------
+ *
+ * The rx-buffer is a ring buffer used to store the received data for each
+ * individual member. This is only the case if data needs to be dejittered
+ * or in case of a conference where different clocks require reclocking.
+ * The transmit-clock (R) will read the buffer.
+ * If the clock overruns the write-pointer, we will have a buffer underrun.
+ * If the write pointer always has a certain distance from the transmit-
+ * clock, we will have a delay. The delay will dynamically be increased and
+ * reduced.
+ *
+ *
+ * TX-Buffer
+ *                  R        W
+ *                  |        |
+ * -----------------+--------+-----------------------
+ *
+ * The tx-buffer is a ring buffer to queue the transmit data from user space
+ * until it will be mixed or sent. There are two pointers, R and W. If the write
+ * pointer W would reach or overrun R, the buffer would overrun. In this case
+ * (some) data is dropped so that it will not overrun.
+ * Additionally a dynamic dejittering can be enabled. this allows data from
+ * user space that have jitter and different clock source.
+ *
+ *
+ * Clock:
+ *
+ * A Clock is not required, if the data source has exactly one clock. In this
+ * case the data source is forwarded to the destination.
+ *
+ * A Clock is required, because the data source
+ *  - has multiple clocks.
+ *  - has no usable clock due to jitter or packet loss (VoIP).
+ * In this case the system's clock is used. The clock resolution depends on
+ * the jiffie resolution.
+ *
+ * If a member joins a conference:
+ *
+ * - If a member joins, its rx_buff is set to silence and change read pointer
+ *   to transmit clock.
+ *
+ * The procedure of received data from card is explained in cmx_receive.
+ * The procedure of received data from user space is explained in cmx_transmit.
+ * The procedure of transmit data to card is cmx_send.
+ *
+ *
+ * Interaction with other features:
+ *
+ * DTMF:
+ * DTMF decoding is done before the data is crossconnected.
+ *
+ * Volume change:
+ * Changing rx-volume is done before the data is crossconnected. The tx-volume
+ * must be changed whenever data is transmitted to the card by the cmx.
+ *
+ * Tones:
+ * If a tone is enabled, it will be processed whenever data is transmitted to
+ * the card. It will replace the tx-data from the user space.
+ * If tones are generated by hardware, this conference member is removed for
+ * this time.
+ *
+ * Disable rx-data:
+ * If cmx is realized in hardware, rx data will be disabled if requested by
+ * the upper layer. If dtmf decoding is done by software and enabled, rx data
+ * will not be diabled but blocked to the upper layer.
+ *
+ * HFC conference engine:
+ * If it is possible to realize all features using hardware, hardware will be
+ * used if not forbidden by control command. Disabling rx-data provides
+ * absolutely traffic free audio processing. (except for the quick 1-frame
+ * upload of a tone loop, only once for a new tone)
+ *
+ */
+
+/* delay.h is required for hw_lock.h */
+
+#include <linux/delay.h>
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "core.h"
+#include "dsp.h"
+/*
+ * debugging of multi party conference,
+ * by using conference even with two members
+ */
+
+/* #define CMX_CONF_DEBUG */
+
+/*#define CMX_DEBUG * massive read/write pointer output */
+/*#define CMX_TX_DEBUG * massive read/write on tx-buffer with content */
+
+static inline int
+count_list_member(struct list_head *head)
+{
+	int			cnt = 0;
+	struct list_head	*m;
+
+	list_for_each(m, head)
+		cnt++;
+	return cnt;
+}
+
+/*
+ * debug cmx memory structure
+ */
+void
+dsp_cmx_debug(struct dsp *dsp)
+{
+	struct dsp_conf	*conf;
+	struct dsp_conf_member	*member;
+	struct dsp		*odsp;
+
+	printk(KERN_DEBUG "-----Current DSP\n");
+	list_for_each_entry(odsp, &dsp_ilist, list) {
+		printk(KERN_DEBUG "* %s echo=%d txmix=%d",
+		    odsp->name, odsp->echo, odsp->tx_mix);
+		if (odsp->conf)
+			printk(" (Conf %d)", odsp->conf->id);
+		if (dsp == odsp)
+			printk(" *this*");
+		printk("\n");
+	}
+	printk(KERN_DEBUG "-----Current Conf:\n");
+	list_for_each_entry(conf, &conf_ilist, list) {
+		printk(KERN_DEBUG "* Conf %d (%p)\n", conf->id, conf);
+		list_for_each_entry(member, &conf->mlist, list) {
+			printk(KERN_DEBUG
+			    "  - member = %s (slot_tx %d, bank_tx %d, "
+			    "slot_rx %d, bank_rx %d hfc_conf %d)%s\n",
+			    member->dsp->name, member->dsp->pcm_slot_tx,
+			    member->dsp->pcm_bank_tx, member->dsp->pcm_slot_rx,
+			    member->dsp->pcm_bank_rx, member->dsp->hfc_conf,
+			    (member->dsp == dsp) ? " *this*" : "");
+		}
+	}
+	printk(KERN_DEBUG "-----end\n");
+}
+
+/*
+ * search conference
+ */
+static struct dsp_conf *
+dsp_cmx_search_conf(u32 id)
+{
+	struct dsp_conf *conf;
+
+	if (!id) {
+		printk(KERN_WARNING "%s: conference ID is 0.\n", __func__);
+		return NULL;
+	}
+
+	/* search conference */
+	list_for_each_entry(conf, &conf_ilist, list)
+		if (conf->id == id)
+			return conf;
+
+	return NULL;
+}
+
+
+/*
+ * add member to conference
+ */
+static int
+dsp_cmx_add_conf_member(struct dsp *dsp, struct dsp_conf *conf)
+{
+	struct dsp_conf_member *member;
+
+	if (!conf || !dsp) {
+		printk(KERN_WARNING "%s: conf or dsp is 0.\n", __func__);
+		return -EINVAL;
+	}
+	if (dsp->member) {
+		printk(KERN_WARNING "%s: dsp is already member in a conf.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (dsp->conf) {
+		printk(KERN_WARNING "%s: dsp is already in a conf.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	member = kzalloc(sizeof(struct dsp_conf_member), GFP_ATOMIC);
+	if (!member) {
+		printk(KERN_ERR "kmalloc struct dsp_conf_member failed\n");
+		return -ENOMEM;
+	}
+	member->dsp = dsp;
+	/* clear rx buffer */
+	memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff));
+	dsp->rx_init = 1; /* rx_W and rx_R will be adjusted on first frame */
+	dsp->rx_W = 0;
+	dsp->rx_R = 0;
+
+	list_add_tail(&member->list, &conf->mlist);
+
+	dsp->conf = conf;
+	dsp->member = member;
+
+	return 0;
+}
+
+
+/*
+ * del member from conference
+ */
+int
+dsp_cmx_del_conf_member(struct dsp *dsp)
+{
+	struct dsp_conf_member *member;
+
+	if (!dsp) {
+		printk(KERN_WARNING "%s: dsp is 0.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (!dsp->conf) {
+		printk(KERN_WARNING "%s: dsp is not in a conf.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (list_empty(&dsp->conf->mlist)) {
+		printk(KERN_WARNING "%s: dsp has linked an empty conf.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* find us in conf */
+	list_for_each_entry(member, &dsp->conf->mlist, list) {
+		if (member->dsp == dsp) {
+			list_del(&member->list);
+			dsp->conf = NULL;
+			dsp->member = NULL;
+			kfree(member);
+			return 0;
+		}
+	}
+	printk(KERN_WARNING
+	    "%s: dsp is not present in its own conf_meber list.\n",
+	    __func__);
+
+	return -EINVAL;
+}
+
+
+/*
+ * new conference
+ */
+static struct dsp_conf
+*dsp_cmx_new_conf(u32 id)
+{
+	struct dsp_conf *conf;
+
+	if (!id) {
+		printk(KERN_WARNING "%s: id is 0.\n",
+		    __func__);
+		return NULL;
+	}
+
+	conf = kzalloc(sizeof(struct dsp_conf), GFP_ATOMIC);
+	if (!conf) {
+		printk(KERN_ERR "kmalloc struct dsp_conf failed\n");
+		return NULL;
+	}
+	INIT_LIST_HEAD(&conf->mlist);
+	conf->id = id;
+
+	list_add_tail(&conf->list, &conf_ilist);
+
+	return conf;
+}
+
+
+/*
+ * del conference
+ */
+int
+dsp_cmx_del_conf(struct dsp_conf *conf)
+{
+	if (!conf) {
+		printk(KERN_WARNING "%s: conf is null.\n",
+		    __func__);
+		return -EINVAL;
+	}
+
+	if (!list_empty(&conf->mlist)) {
+		printk(KERN_WARNING "%s: conf not empty.\n",
+		    __func__);
+		return -EINVAL;
+	}
+	list_del(&conf->list);
+	kfree(conf);
+
+	return 0;
+}
+
+
+/*
+ * send HW message to hfc card
+ */
+static void
+dsp_cmx_hw_message(struct dsp *dsp, u32 message, u32 param1, u32 param2,
+    u32 param3, u32 param4)
+{
+	struct mISDN_ctrl_req cq;
+
+	memset(&cq, 0, sizeof(cq));
+	cq.op = message;
+	cq.p1 = param1 | (param2 << 8);
+	cq.p2 = param3 | (param4 << 8);
+	if (dsp->ch.peer)
+		dsp->ch.peer->ctrl(dsp->ch.peer, CONTROL_CHANNEL, &cq);
+}
+
+
+/*
+ * do hardware update and set the software/hardware flag
+ *
+ * either a conference or a dsp instance can be given
+ * if only dsp instance is given, the instance is not associated with a conf
+ * and therefore removed. if a conference is given, the dsp is expected to
+ * be member of that conference.
+ */
+void
+dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp)
+{
+	struct dsp_conf_member	*member, *nextm;
+	struct dsp		*finddsp;
+	int		memb = 0, i, ii, i1, i2;
+	int		freeunits[8];
+	u_char		freeslots[256];
+	int		same_hfc = -1, same_pcm = -1, current_conf = -1,
+	    all_conf = 1;
+
+	/* dsp gets updated (no conf) */
+	if (!conf) {
+		if (!dsp)
+			return;
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG "%s checking dsp %s\n",
+			    __func__, dsp->name);
+one_member:
+		/* remove HFC conference if enabled */
+		if (dsp->hfc_conf >= 0) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s removing %s from HFC conf %d "
+				    "because dsp is split\n", __func__,
+				    dsp->name, dsp->hfc_conf);
+			dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_CONF_SPLIT,
+			    0, 0, 0, 0);
+			dsp->hfc_conf = -1;
+		}
+		/* process hw echo */
+		if (dsp->features.pcm_banks < 1)
+			return;
+		if (!dsp->echo) {
+			/* NO ECHO: remove PCM slot if assigned */
+			if (dsp->pcm_slot_tx >= 0 || dsp->pcm_slot_rx >= 0) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG "%s removing %s from"
+					    " PCM slot %d (TX) %d (RX) because"
+					    " dsp is split (no echo)\n",
+					    __func__, dsp->name,
+					    dsp->pcm_slot_tx, dsp->pcm_slot_rx);
+				dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_DISC,
+				    0, 0, 0, 0);
+				dsp->pcm_slot_tx = -1;
+				dsp->pcm_bank_tx = -1;
+				dsp->pcm_slot_rx = -1;
+				dsp->pcm_bank_rx = -1;
+			}
+			return;
+		}
+		/* ECHO: already echo */
+		if (dsp->pcm_slot_tx >= 0 && dsp->pcm_slot_rx < 0 &&
+		    dsp->pcm_bank_tx == 2 && dsp->pcm_bank_rx == 2)
+			return;
+		/* ECHO: if slot already assigned */
+		if (dsp->pcm_slot_tx >= 0) {
+			dsp->pcm_slot_rx = dsp->pcm_slot_tx;
+			dsp->pcm_bank_tx = 2; /* 2 means loop */
+			dsp->pcm_bank_rx = 2;
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s refresh %s for echo using slot %d\n",
+				    __func__, dsp->name,
+				    dsp->pcm_slot_tx);
+			dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    dsp->pcm_slot_tx, 2, dsp->pcm_slot_rx, 2);
+			return;
+		}
+		/* ECHO: find slot */
+		dsp->pcm_slot_tx = -1;
+		dsp->pcm_slot_rx = -1;
+		memset(freeslots, 1, sizeof(freeslots));
+		list_for_each_entry(finddsp, &dsp_ilist, list) {
+			if (finddsp->features.pcm_id == dsp->features.pcm_id) {
+				if (finddsp->pcm_slot_rx >= 0 &&
+				    finddsp->pcm_slot_rx < sizeof(freeslots))
+					freeslots[finddsp->pcm_slot_tx] = 0;
+				if (finddsp->pcm_slot_tx >= 0 &&
+				    finddsp->pcm_slot_tx < sizeof(freeslots))
+					freeslots[finddsp->pcm_slot_rx] = 0;
+			}
+		}
+		i = 0;
+		ii = dsp->features.pcm_slots;
+		while (i < ii) {
+			if (freeslots[i])
+				break;
+			i++;
+		}
+		if (i == ii) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s no slot available for echo\n",
+				    __func__);
+			/* no more slots available */
+			return;
+		}
+		/* assign free slot */
+		dsp->pcm_slot_tx = i;
+		dsp->pcm_slot_rx = i;
+		dsp->pcm_bank_tx = 2; /* loop */
+		dsp->pcm_bank_rx = 2;
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "%s assign echo for %s using slot %d\n",
+			    __func__, dsp->name, dsp->pcm_slot_tx);
+		dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_CONN,
+		    dsp->pcm_slot_tx, 2, dsp->pcm_slot_rx, 2);
+		return;
+	}
+
+	/* conf gets updated (all members) */
+	if (dsp_debug & DEBUG_DSP_CMX)
+		printk(KERN_DEBUG "%s checking conference %d\n",
+		    __func__, conf->id);
+
+	if (list_empty(&conf->mlist)) {
+		printk(KERN_ERR "%s: conference whithout members\n",
+		    __func__);
+		return;
+	}
+	member = list_entry(conf->mlist.next, struct dsp_conf_member, list);
+	same_hfc = member->dsp->features.hfc_id;
+	same_pcm = member->dsp->features.pcm_id;
+	/* check all members in our conference */
+	list_for_each_entry(member, &conf->mlist, list) {
+		/* check if member uses mixing */
+		if (member->dsp->tx_mix) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "tx_mix is turned on\n", __func__,
+				    member->dsp->name);
+conf_software:
+			list_for_each_entry(member, &conf->mlist, list) {
+				dsp = member->dsp;
+				/* remove HFC conference if enabled */
+				if (dsp->hfc_conf >= 0) {
+					if (dsp_debug & DEBUG_DSP_CMX)
+						printk(KERN_DEBUG
+						    "%s removing %s from HFC "
+						    "conf %d because not "
+						    "possible with hardware\n",
+						    __func__,
+						    dsp->name,
+						    dsp->hfc_conf);
+					dsp_cmx_hw_message(dsp,
+					    MISDN_CTRL_HFC_CONF_SPLIT,
+					    0, 0, 0, 0);
+					dsp->hfc_conf = -1;
+				}
+				/* remove PCM slot if assigned */
+				if (dsp->pcm_slot_tx >= 0 ||
+				    dsp->pcm_slot_rx >= 0) {
+					if (dsp_debug & DEBUG_DSP_CMX)
+						printk(KERN_DEBUG "%s removing "
+						    "%s from PCM slot %d (TX)"
+						    " slot %d (RX) because not"
+						    " possible with hardware\n",
+						    __func__,
+						    dsp->name,
+						    dsp->pcm_slot_tx,
+						    dsp->pcm_slot_rx);
+					dsp_cmx_hw_message(dsp,
+					    MISDN_CTRL_HFC_PCM_DISC,
+					    0, 0, 0, 0);
+					dsp->pcm_slot_tx = -1;
+					dsp->pcm_bank_tx = -1;
+					dsp->pcm_slot_rx = -1;
+					dsp->pcm_bank_rx = -1;
+				}
+			}
+			conf->hardware = 0;
+			conf->software = 1;
+			return;
+		}
+		/* check if member has echo turned on */
+		if (member->dsp->echo) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "echo is turned on\n", __func__,
+				    member->dsp->name);
+			goto conf_software;
+		}
+		/* check if member has tx_mix turned on */
+		if (member->dsp->tx_mix) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "tx_mix is turned on\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* check if member changes volume at an not suppoted level */
+		if (member->dsp->tx_volume) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "tx_volume is changed\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		if (member->dsp->rx_volume) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "rx_volume is changed\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* check if tx-data turned on */
+		if (member->dsp->tx_data) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "tx_data is turned on\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* check if pipeline exists */
+		if (member->dsp->pipeline.inuse) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "pipeline exists\n", __func__,
+				    member->dsp->name);
+			goto conf_software;
+		}
+		/* check if encryption is enabled */
+		if (member->dsp->bf_enable) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG "%s dsp %s cannot form a "
+				    "conf, because encryption is enabled\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* check if member is on a card with PCM support */
+		if (member->dsp->features.pcm_id < 0) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "dsp has no PCM bus\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* check if relations are on the same PCM bus */
+		if (member->dsp->features.pcm_id != same_pcm) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s dsp %s cannot form a conf, because "
+				    "dsp is on a different PCM bus than the "
+				    "first dsp\n",
+				    __func__, member->dsp->name);
+			goto conf_software;
+		}
+		/* determine if members are on the same hfc chip */
+		if (same_hfc != member->dsp->features.hfc_id)
+			same_hfc = -1;
+		/* if there are members already in a conference */
+		if (current_conf < 0 && member->dsp->hfc_conf >= 0)
+			current_conf = member->dsp->hfc_conf;
+		/* if any member is not in a conference */
+		if (member->dsp->hfc_conf < 0)
+			all_conf = 0;
+
+		memb++;
+	}
+
+	/* if no member, this is an error */
+	if (memb < 1)
+		return;
+
+	/* one member */
+	if (memb == 1) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "%s conf %d cannot form a HW conference, "
+			    "because dsp is alone\n", __func__, conf->id);
+		conf->hardware = 0;
+		conf->software = 0;
+		member = list_entry(conf->mlist.next, struct dsp_conf_member,
+			list);
+		dsp = member->dsp;
+		goto one_member;
+	}
+
+	/*
+	 * ok, now we are sure that all members are on the same pcm.
+	 * now we will see if we have only two members, so we can do
+	 * crossconnections, which don't have any limitations.
+	 */
+
+	/* if we have only two members */
+	if (memb == 2) {
+		member = list_entry(conf->mlist.next, struct dsp_conf_member,
+			list);
+		nextm = list_entry(member->list.next, struct dsp_conf_member,
+			list);
+		/* remove HFC conference if enabled */
+		if (member->dsp->hfc_conf >= 0) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s removing %s from HFC conf %d because "
+				    "two parties require only a PCM slot\n",
+				    __func__, member->dsp->name,
+				    member->dsp->hfc_conf);
+			dsp_cmx_hw_message(member->dsp,
+			    MISDN_CTRL_HFC_CONF_SPLIT, 0, 0, 0, 0);
+			member->dsp->hfc_conf = -1;
+		}
+		if (nextm->dsp->hfc_conf >= 0) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s removing %s from HFC conf %d because "
+				    "two parties require only a PCM slot\n",
+				    __func__, nextm->dsp->name,
+				    nextm->dsp->hfc_conf);
+			dsp_cmx_hw_message(nextm->dsp,
+			    MISDN_CTRL_HFC_CONF_SPLIT, 0, 0, 0, 0);
+			nextm->dsp->hfc_conf = -1;
+		}
+		/* if members have two banks (and not on the same chip) */
+		if (member->dsp->features.pcm_banks > 1 &&
+		    nextm->dsp->features.pcm_banks > 1 &&
+		    member->dsp->features.hfc_id !=
+		    nextm->dsp->features.hfc_id) {
+			/* if both members have same slots with crossed banks */
+			if (member->dsp->pcm_slot_tx >= 0 &&
+			    member->dsp->pcm_slot_rx >= 0 &&
+			    nextm->dsp->pcm_slot_tx >= 0 &&
+			    nextm->dsp->pcm_slot_rx >= 0 &&
+			    nextm->dsp->pcm_slot_tx ==
+			    member->dsp->pcm_slot_rx &&
+			    nextm->dsp->pcm_slot_rx ==
+			    member->dsp->pcm_slot_tx &&
+			    nextm->dsp->pcm_slot_tx ==
+			    member->dsp->pcm_slot_tx &&
+			    member->dsp->pcm_bank_tx !=
+			    member->dsp->pcm_bank_rx &&
+			    nextm->dsp->pcm_bank_tx !=
+			    nextm->dsp->pcm_bank_rx) {
+				/* all members have same slot */
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s dsp %s & %s stay joined on "
+					    "PCM slot %d bank %d (TX) bank %d "
+					    "(RX) (on different chips)\n",
+					    __func__,
+					    member->dsp->name,
+					    nextm->dsp->name,
+					    member->dsp->pcm_slot_tx,
+					    member->dsp->pcm_bank_tx,
+					    member->dsp->pcm_bank_rx);
+				conf->hardware = 0;
+				conf->software = 1;
+				return;
+			}
+			/* find a new slot */
+			memset(freeslots, 1, sizeof(freeslots));
+			list_for_each_entry(dsp, &dsp_ilist, list) {
+				if (dsp != member->dsp &&
+				    dsp != nextm->dsp &&
+				    member->dsp->features.pcm_id ==
+				    dsp->features.pcm_id) {
+					if (dsp->pcm_slot_rx >= 0 &&
+					    dsp->pcm_slot_rx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_tx] = 0;
+					if (dsp->pcm_slot_tx >= 0 &&
+					    dsp->pcm_slot_tx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_rx] = 0;
+				}
+			}
+			i = 0;
+			ii = member->dsp->features.pcm_slots;
+			while (i < ii) {
+				if (freeslots[i])
+					break;
+				i++;
+			}
+			if (i == ii) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s no slot available for "
+					    "%s & %s\n", __func__,
+					    member->dsp->name,
+					    nextm->dsp->name);
+				/* no more slots available */
+				goto conf_software;
+			}
+			/* assign free slot */
+			member->dsp->pcm_slot_tx = i;
+			member->dsp->pcm_slot_rx = i;
+			nextm->dsp->pcm_slot_tx = i;
+			nextm->dsp->pcm_slot_rx = i;
+			member->dsp->pcm_bank_rx = 0;
+			member->dsp->pcm_bank_tx = 1;
+			nextm->dsp->pcm_bank_rx = 1;
+			nextm->dsp->pcm_bank_tx = 0;
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s adding %s & %s to new PCM slot %d "
+				    "(TX and RX on different chips) because "
+				    "both members have not same slots\n",
+				    __func__,
+				    member->dsp->name,
+				    nextm->dsp->name,
+				    member->dsp->pcm_slot_tx);
+			dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    member->dsp->pcm_slot_tx, member->dsp->pcm_bank_tx,
+			    member->dsp->pcm_slot_rx, member->dsp->pcm_bank_rx);
+			dsp_cmx_hw_message(nextm->dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    nextm->dsp->pcm_slot_tx, nextm->dsp->pcm_bank_tx,
+			    nextm->dsp->pcm_slot_rx, nextm->dsp->pcm_bank_rx);
+			conf->hardware = 1;
+			conf->software = 0;
+			return;
+		/* if members have one bank (or on the same chip) */
+		} else {
+			/* if both members have different crossed slots */
+			if (member->dsp->pcm_slot_tx >= 0 &&
+			    member->dsp->pcm_slot_rx >= 0 &&
+			    nextm->dsp->pcm_slot_tx >= 0 &&
+			    nextm->dsp->pcm_slot_rx >= 0 &&
+			    nextm->dsp->pcm_slot_tx ==
+			    member->dsp->pcm_slot_rx &&
+			    nextm->dsp->pcm_slot_rx ==
+			    member->dsp->pcm_slot_tx &&
+			    member->dsp->pcm_slot_tx !=
+			    member->dsp->pcm_slot_rx &&
+			    member->dsp->pcm_bank_tx == 0 &&
+			    member->dsp->pcm_bank_rx == 0 &&
+			    nextm->dsp->pcm_bank_tx == 0 &&
+			    nextm->dsp->pcm_bank_rx == 0) {
+				/* all members have same slot */
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s dsp %s & %s stay joined on PCM "
+					    "slot %d (TX) %d (RX) on same chip "
+					    "or one bank PCM)\n", __func__,
+					    member->dsp->name,
+					    nextm->dsp->name,
+					    member->dsp->pcm_slot_tx,
+					    member->dsp->pcm_slot_rx);
+				conf->hardware = 0;
+				conf->software = 1;
+				return;
+			}
+			/* find two new slot */
+			memset(freeslots, 1, sizeof(freeslots));
+			list_for_each_entry(dsp, &dsp_ilist, list) {
+				if (dsp != member->dsp &&
+				    dsp != nextm->dsp &&
+				    member->dsp->features.pcm_id ==
+				    dsp->features.pcm_id) {
+					if (dsp->pcm_slot_rx >= 0 &&
+					    dsp->pcm_slot_rx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_tx] = 0;
+					if (dsp->pcm_slot_tx >= 0 &&
+					    dsp->pcm_slot_tx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_rx] = 0;
+				}
+			}
+			i1 = 0;
+			ii = member->dsp->features.pcm_slots;
+			while (i1 < ii) {
+				if (freeslots[i1])
+					break;
+				i1++;
+			}
+			if (i1 == ii) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s no slot available "
+					    "for %s & %s\n", __func__,
+					    member->dsp->name,
+					    nextm->dsp->name);
+				/* no more slots available */
+				goto conf_software;
+			}
+			i2 = i1+1;
+			while (i2 < ii) {
+				if (freeslots[i2])
+					break;
+				i2++;
+			}
+			if (i2 == ii) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s no slot available "
+					    "for %s & %s\n",
+					    __func__,
+					    member->dsp->name,
+					    nextm->dsp->name);
+				/* no more slots available */
+				goto conf_software;
+			}
+			/* assign free slots */
+			member->dsp->pcm_slot_tx = i1;
+			member->dsp->pcm_slot_rx = i2;
+			nextm->dsp->pcm_slot_tx = i2;
+			nextm->dsp->pcm_slot_rx = i1;
+			member->dsp->pcm_bank_rx = 0;
+			member->dsp->pcm_bank_tx = 0;
+			nextm->dsp->pcm_bank_rx = 0;
+			nextm->dsp->pcm_bank_tx = 0;
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s adding %s & %s to new PCM slot %d "
+				    "(TX) %d (RX) on same chip or one bank "
+				    "PCM, because both members have not "
+				    "crossed slots\n", __func__,
+				    member->dsp->name,
+				    nextm->dsp->name,
+				    member->dsp->pcm_slot_tx,
+				    member->dsp->pcm_slot_rx);
+			dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    member->dsp->pcm_slot_tx, member->dsp->pcm_bank_tx,
+			    member->dsp->pcm_slot_rx, member->dsp->pcm_bank_rx);
+			dsp_cmx_hw_message(nextm->dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    nextm->dsp->pcm_slot_tx, nextm->dsp->pcm_bank_tx,
+			    nextm->dsp->pcm_slot_rx, nextm->dsp->pcm_bank_rx);
+			conf->hardware = 1;
+			conf->software = 0;
+			return;
+		}
+	}
+
+	/*
+	 * if we have more than two, we may check if we have a conference
+	 * unit available on the chip. also all members must be on the same
+	 */
+
+	/* if not the same HFC chip */
+	if (same_hfc < 0) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "%s conference %d cannot be formed, because "
+			    "members are on different chips or not "
+			    "on HFC chip\n",
+			    __func__, conf->id);
+		goto conf_software;
+	}
+
+	/* for more than two members.. */
+
+	/* in case of hdlc, we change to software */
+	if (dsp->hdlc)
+		goto conf_software;
+
+	/* if all members already have the same conference */
+	if (all_conf)
+		return;
+
+	/*
+	 * if there is an existing conference, but not all members have joined
+	 */
+	if (current_conf >= 0) {
+join_members:
+		list_for_each_entry(member, &conf->mlist, list) {
+			/* join to current conference */
+			if (member->dsp->hfc_conf == current_conf)
+				continue;
+			/* get a free timeslot first */
+			memset(freeslots, 1, sizeof(freeslots));
+			list_for_each_entry(dsp, &dsp_ilist, list) {
+				/*
+				 * not checking current member, because
+				 * slot will be overwritten.
+				 */
+				if (
+				    dsp != member->dsp &&
+				/* dsp must be on the same PCM */
+				    member->dsp->features.pcm_id ==
+				    dsp->features.pcm_id) {
+					/* dsp must be on a slot */
+					if (dsp->pcm_slot_tx >= 0 &&
+					    dsp->pcm_slot_tx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_tx] = 0;
+					if (dsp->pcm_slot_rx >= 0 &&
+					    dsp->pcm_slot_rx <
+					    sizeof(freeslots))
+						freeslots[dsp->pcm_slot_rx] = 0;
+				}
+			}
+			i = 0;
+			ii = member->dsp->features.pcm_slots;
+			while (i < ii) {
+				if (freeslots[i])
+					break;
+				i++;
+			}
+			if (i == ii) {
+				/* no more slots available */
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s conference %d cannot be formed,"
+					    " because no slot free\n",
+					    __func__, conf->id);
+				goto conf_software;
+			}
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "%s changing dsp %s to HW conference "
+				    "%d slot %d\n", __func__,
+				    member->dsp->name, current_conf, i);
+			/* assign free slot & set PCM & join conf */
+			member->dsp->pcm_slot_tx = i;
+			member->dsp->pcm_slot_rx = i;
+			member->dsp->pcm_bank_tx = 2; /* loop */
+			member->dsp->pcm_bank_rx = 2;
+			member->dsp->hfc_conf = current_conf;
+			dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN,
+			    i, 2, i, 2);
+			dsp_cmx_hw_message(member->dsp,
+			    MISDN_CTRL_HFC_CONF_JOIN, current_conf, 0, 0, 0);
+		}
+		return;
+	}
+
+	/*
+	 * no member is in a conference yet, so we find a free one
+	 */
+	memset(freeunits, 1, sizeof(freeunits));
+	list_for_each_entry(dsp, &dsp_ilist, list) {
+		/* dsp must be on the same chip */
+		if (dsp->features.hfc_id == same_hfc &&
+		    /* dsp must have joined a HW conference */
+		    dsp->hfc_conf >= 0 &&
+		    /* slot must be within range */
+		    dsp->hfc_conf < 8)
+			freeunits[dsp->hfc_conf] = 0;
+	}
+	i = 0;
+	ii = 8;
+	while (i < ii) {
+		if (freeunits[i])
+			break;
+		i++;
+	}
+	if (i == ii) {
+		/* no more conferences available */
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "%s conference %d cannot be formed, because "
+			    "no conference number free\n",
+			    __func__, conf->id);
+		goto conf_software;
+	}
+	/* join all members */
+	current_conf = i;
+	goto join_members;
+}
+
+
+/*
+ * conf_id != 0: join or change conference
+ * conf_id == 0: split from conference if not already
+ */
+int
+dsp_cmx_conf(struct dsp *dsp, u32 conf_id)
+{
+	int err;
+	struct dsp_conf *conf;
+	struct dsp_conf_member	*member;
+
+	/* if conference doesn't change */
+	if (dsp->conf_id == conf_id)
+		return 0;
+
+	/* first remove us from current conf */
+	if (dsp->conf_id) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG "removing us from conference %d\n",
+				dsp->conf->id);
+		/* remove us from conf */
+		conf = dsp->conf;
+		err = dsp_cmx_del_conf_member(dsp);
+		if (err)
+			return err;
+		dsp->conf_id = 0;
+
+		/* update hardware */
+		dsp_cmx_hardware(NULL, dsp);
+
+		/* conf now empty? */
+		if (list_empty(&conf->mlist)) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "conference is empty, so we remove it.\n");
+			err = dsp_cmx_del_conf(conf);
+			if (err)
+				return err;
+		} else {
+			/* update members left on conf */
+			dsp_cmx_hardware(conf, NULL);
+		}
+	}
+
+	/* if split */
+	if (!conf_id)
+		return 0;
+
+	/* now add us to conf */
+	if (dsp_debug & DEBUG_DSP_CMX)
+		printk(KERN_DEBUG "searching conference %d\n",
+			conf_id);
+	conf = dsp_cmx_search_conf(conf_id);
+	if (!conf) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "conference doesn't exist yet, creating.\n");
+		/* the conference doesn't exist, so we create */
+		conf = dsp_cmx_new_conf(conf_id);
+		if (!conf)
+			return -EINVAL;
+	} else if (!list_empty(&conf->mlist)) {
+		member = list_entry(conf->mlist.next, struct dsp_conf_member,
+			list);
+		if (dsp->hdlc && !member->dsp->hdlc) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "cannot join transparent conference.\n");
+			return -EINVAL;
+		}
+		if (!dsp->hdlc && member->dsp->hdlc) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "cannot join hdlc conference.\n");
+			return -EINVAL;
+		}
+	}
+	/* add conference member */
+	err = dsp_cmx_add_conf_member(dsp, conf);
+	if (err)
+		return err;
+	dsp->conf_id = conf_id;
+
+	/* if we are alone, we do nothing! */
+	if (list_empty(&conf->mlist)) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "we are alone in this conference, so exit.\n");
+		/* update hardware */
+		dsp_cmx_hardware(NULL, dsp);
+		return 0;
+	}
+
+	/* update members on conf */
+	dsp_cmx_hardware(conf, NULL);
+
+	return 0;
+}
+
+
+/*
+ * audio data is received from card
+ */
+void
+dsp_cmx_receive(struct dsp *dsp, struct sk_buff *skb)
+{
+	u8 *d, *p;
+	int len = skb->len;
+	struct mISDNhead *hh = mISDN_HEAD_P(skb);
+	int w, i, ii;
+
+	/* check if we have sompen */
+	if (len < 1)
+		return;
+
+	/* half of the buffer should be larger than maximum packet size */
+	if (len >= CMX_BUFF_HALF) {
+		printk(KERN_ERR
+		    "%s line %d: packet from card is too large (%d bytes). "
+		    "please make card send smaller packets OR increase "
+		    "CMX_BUFF_SIZE\n", __FILE__, __LINE__, len);
+		return;
+	}
+
+	/*
+	 * initialize pointers if not already -
+	 * also add delay if requested by PH_SIGNAL
+	 */
+	if (dsp->rx_init) {
+		dsp->rx_init = 0;
+		if (dsp->features.unordered) {
+			dsp->rx_R = (hh->id & CMX_BUFF_MASK);
+			dsp->rx_W = (dsp->rx_R + dsp->cmx_delay)
+				& CMX_BUFF_MASK;
+		} else {
+			dsp->rx_R = 0;
+			dsp->rx_W = dsp->cmx_delay;
+		}
+	}
+	/* if frame contains time code, write directly */
+	if (dsp->features.unordered) {
+		dsp->rx_W = (hh->id & CMX_BUFF_MASK);
+		/* printk(KERN_DEBUG "%s %08x\n", dsp->name, hh->id); */
+	}
+	/*
+	 * if we underrun (or maybe overrun),
+	 * we set our new read pointer, and write silence to buffer
+	 */
+	if (((dsp->rx_W-dsp->rx_R) & CMX_BUFF_MASK) >= CMX_BUFF_HALF) {
+		if (dsp_debug & DEBUG_DSP_CMX)
+			printk(KERN_DEBUG
+			    "cmx_receive(dsp=%lx): UNDERRUN (or overrun the "
+			    "maximum delay), adjusting read pointer! "
+			    "(inst %s)\n", (u_long)dsp, dsp->name);
+		/* flush buffer */
+		if (dsp->features.unordered) {
+			dsp->rx_R = (hh->id & CMX_BUFF_MASK);
+			dsp->rx_W = (dsp->rx_R + dsp->cmx_delay)
+				& CMX_BUFF_MASK;
+		} else {
+			dsp->rx_R = 0;
+			dsp->rx_W = dsp->cmx_delay;
+		}
+		memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff));
+	}
+	/* if we have reached double delay, jump back to middle */
+	if (dsp->cmx_delay)
+		if (((dsp->rx_W - dsp->rx_R) & CMX_BUFF_MASK) >=
+		    (dsp->cmx_delay << 1)) {
+			if (dsp_debug & DEBUG_DSP_CMX)
+				printk(KERN_DEBUG
+				    "cmx_receive(dsp=%lx): OVERRUN (because "
+				    "twice the delay is reached), adjusting "
+				    "read pointer! (inst %s)\n",
+				    (u_long)dsp, dsp->name);
+		/* flush buffer */
+		if (dsp->features.unordered) {
+			dsp->rx_R = (hh->id & CMX_BUFF_MASK);
+			dsp->rx_W = (dsp->rx_R + dsp->cmx_delay)
+				& CMX_BUFF_MASK;
+		} else {
+			dsp->rx_R = 0;
+			dsp->rx_W = dsp->cmx_delay;
+		}
+		memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff));
+	}
+
+	/* show where to write */
+#ifdef CMX_DEBUG
+	printk(KERN_DEBUG
+	    "cmx_receive(dsp=%lx): rx_R(dsp)=%05x rx_W(dsp)=%05x len=%d %s\n",
+	    (u_long)dsp, dsp->rx_R, dsp->rx_W, len, dsp->name);
+#endif
+
+	/* write data into rx_buffer */
+	p = skb->data;
+	d = dsp->rx_buff;
+	w = dsp->rx_W;
+	i = 0;
+	ii = len;
+	while (i < ii) {
+		d[w++ & CMX_BUFF_MASK] = *p++;
+		i++;
+	}
+
+	/* increase write-pointer */
+	dsp->rx_W = ((dsp->rx_W+len) & CMX_BUFF_MASK);
+}
+
+
+/*
+ * send (mixed) audio data to card and control jitter
+ */
+static void
+dsp_cmx_send_member(struct dsp *dsp, int len, s32 *c, int members)
+{
+	struct dsp_conf *conf = dsp->conf;
+	struct dsp *member, *other;
+	register s32 sample;
+	u8 *d, *p, *q, *o_q;
+	struct sk_buff *nskb, *txskb;
+	int r, rr, t, tt, o_r, o_rr;
+	int preload = 0;
+	struct mISDNhead *hh, *thh;
+
+	/* don't process if: */
+	if (!dsp->b_active) { /* if not active */
+		dsp->last_tx = 0;
+		return;
+	}
+	if (dsp->pcm_slot_tx >= 0 && /* connected to pcm slot */
+	    dsp->tx_R == dsp->tx_W && /* AND no tx-data */
+	    !(dsp->tone.tone && dsp->tone.software)) { /* AND not soft tones */
+		dsp->last_tx = 0;
+		return;
+	}
+
+#ifdef CMX_DEBUG
+	printk(KERN_DEBUG
+	    "SEND members=%d dsp=%s, conf=%p, rx_R=%05x rx_W=%05x\n",
+	    members, dsp->name, conf, dsp->rx_R, dsp->rx_W);
+#endif
+
+	/* preload if we have delay set */
+	if (dsp->cmx_delay && !dsp->last_tx) {
+		preload = len;
+		if (preload < 128)
+			preload = 128;
+	}
+
+	/* PREPARE RESULT */
+	nskb = mI_alloc_skb(len + preload, GFP_ATOMIC);
+	if (!nskb) {
+		printk(KERN_ERR
+		    "FATAL ERROR in mISDN_dsp.o: cannot alloc %d bytes\n",
+		    len + preload);
+		return;
+	}
+	hh = mISDN_HEAD_P(nskb);
+	hh->prim = PH_DATA_REQ;
+	hh->id = 0;
+	dsp->last_tx = 1;
+
+	/* set pointers, indexes and stuff */
+	member = dsp;
+	p = dsp->tx_buff; /* transmit data */
+	q = dsp->rx_buff; /* received data */
+	d = skb_put(nskb, preload + len); /* result */
+	t = dsp->tx_R; /* tx-pointers */
+	tt = dsp->tx_W;
+	r = dsp->rx_R; /* rx-pointers */
+	rr = (r + len) & CMX_BUFF_MASK;
+
+	/* preload with silence, if required */
+	if (preload) {
+		memset(d, dsp_silence, preload);
+		d += preload;
+	}
+
+	/* PROCESS TONES/TX-DATA ONLY */
+	if (dsp->tone.tone && dsp->tone.software) {
+		/* -> copy tone */
+		dsp_tone_copy(dsp, d, len);
+		dsp->tx_R = 0; /* clear tx buffer */
+		dsp->tx_W = 0;
+		goto send_packet;
+	}
+	/* if we have tx-data but do not use mixing */
+	if (!dsp->tx_mix && t != tt) {
+		/* -> send tx-data and continue when not enough */
+#ifdef CMX_TX_DEBUG
+	sprintf(debugbuf, "TX sending (%04x-%04x)%p: ", t, tt, p);
+#endif
+		while (r != rr && t != tt) {
+#ifdef CMX_TX_DEBUG
+			if (strlen(debugbuf) < 48)
+			    sprintf(debugbuf+strlen(debugbuf), " %02x", p[t]);
+#endif
+			*d++ = p[t]; /* write tx_buff */
+			t = (t+1) & CMX_BUFF_MASK;
+			r = (r+1) & CMX_BUFF_MASK;
+		}
+		if (r == rr) {
+			dsp->tx_R = t;
+#ifdef CMX_TX_DEBUG
+	printk(KERN_DEBUG "%s\n", debugbuf);
+#endif
+			goto send_packet;
+		}
+	}
+#ifdef CMX_TX_DEBUG
+	printk(KERN_DEBUG "%s\n", debugbuf);
+#endif
+
+	/* PROCESS DATA (one member / no conf) */
+	if (!conf || members <= 1) {
+		/* -> if echo is NOT enabled */
+		if (!dsp->echo) {
+			/* -> send tx-data if available or use 0-volume */
+			while (r != rr && t != tt) {
+				*d++ = p[t]; /* write tx_buff */
+				t = (t+1) & CMX_BUFF_MASK;
+				r = (r+1) & CMX_BUFF_MASK;
+			}
+			if (r != rr)
+				memset(d, dsp_silence, (rr-r)&CMX_BUFF_MASK);
+		/* -> if echo is enabled */
+		} else {
+			/*
+			 * -> mix tx-data with echo if available,
+			 * or use echo only
+			 */
+			while (r != rr && t != tt) {
+				*d++ = dsp_audio_mix_law[(p[t]<<8)|q[r]];
+				t = (t+1) & CMX_BUFF_MASK;
+				r = (r+1) & CMX_BUFF_MASK;
+			}
+			while (r != rr) {
+				*d++ = q[r]; /* echo */
+				r = (r+1) & CMX_BUFF_MASK;
+			}
+		}
+		dsp->tx_R = t;
+		goto send_packet;
+	}
+	/* PROCESS DATA (two members) */
+#ifdef CMX_CONF_DEBUG
+	if (0) {
+#else
+	if (members == 2) {
+#endif
+		/* "other" becomes other party */
+		other = (list_entry(conf->mlist.next,
+		    struct dsp_conf_member, list))->dsp;
+		if (other == member)
+			other = (list_entry(conf->mlist.prev,
+			    struct dsp_conf_member, list))->dsp;
+		o_q = other->rx_buff; /* received data */
+		o_rr = (other->rx_R + len) & CMX_BUFF_MASK;
+			/* end of rx-pointer */
+		o_r = (o_rr - rr + r) & CMX_BUFF_MASK;
+			/* start rx-pointer at current read position*/
+		/* -> if echo is NOT enabled */
+		if (!dsp->echo) {
+			/*
+			 * -> copy other member's rx-data,
+			 * if tx-data is available, mix
+			 */
+			while (o_r != o_rr && t != tt) {
+				*d++ = dsp_audio_mix_law[(p[t]<<8)|o_q[o_r]];
+				t = (t+1) & CMX_BUFF_MASK;
+				o_r = (o_r+1) & CMX_BUFF_MASK;
+			}
+			while (o_r != o_rr) {
+				*d++ = o_q[o_r];
+				o_r = (o_r+1) & CMX_BUFF_MASK;
+			}
+		/* -> if echo is enabled */
+		} else {
+			/*
+			 * -> mix other member's rx-data with echo,
+			 * if tx-data is available, mix
+			 */
+			while (r != rr && t != tt) {
+				sample = dsp_audio_law_to_s32[p[t]] +
+				    dsp_audio_law_to_s32[q[r]] +
+				    dsp_audio_law_to_s32[o_q[o_r]];
+				if (sample < -32768)
+					sample = -32768;
+				else if (sample > 32767)
+					sample = 32767;
+				*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+				    /* tx-data + rx_data + echo */
+				t = (t+1) & CMX_BUFF_MASK;
+				r = (r+1) & CMX_BUFF_MASK;
+				o_r = (o_r+1) & CMX_BUFF_MASK;
+			}
+			while (r != rr) {
+				*d++ = dsp_audio_mix_law[(q[r]<<8)|o_q[o_r]];
+				r = (r+1) & CMX_BUFF_MASK;
+				o_r = (o_r+1) & CMX_BUFF_MASK;
+			}
+		}
+		dsp->tx_R = t;
+		goto send_packet;
+	}
+#ifdef DSP_NEVER_DEFINED
+	}
+#endif
+	/* PROCESS DATA (three or more members) */
+	/* -> if echo is NOT enabled */
+	if (!dsp->echo) {
+		/*
+		 * -> substract rx-data from conf-data,
+		 * if tx-data is available, mix
+		 */
+		while (r != rr && t != tt) {
+			sample = dsp_audio_law_to_s32[p[t]] + *c++ -
+			    dsp_audio_law_to_s32[q[r]];
+			if (sample < -32768)
+				sample = -32768;
+			else if (sample > 32767)
+				sample = 32767;
+			*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+			    /* conf-rx+tx */
+			r = (r+1) & CMX_BUFF_MASK;
+			t = (t+1) & CMX_BUFF_MASK;
+		}
+		while (r != rr) {
+			sample = *c++ - dsp_audio_law_to_s32[q[r]];
+			if (sample < -32768)
+				sample = -32768;
+			else if (sample > 32767)
+				sample = 32767;
+			*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+			    /* conf-rx */
+			r = (r+1) & CMX_BUFF_MASK;
+		}
+	/* -> if echo is enabled */
+	} else {
+		/*
+		 * -> encode conf-data, if tx-data
+		 * is available, mix
+		 */
+		while (r != rr && t != tt) {
+			sample = dsp_audio_law_to_s32[p[t]] + *c++;
+			if (sample < -32768)
+				sample = -32768;
+			else if (sample > 32767)
+				sample = 32767;
+			*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+			    /* conf(echo)+tx */
+			t = (t+1) & CMX_BUFF_MASK;
+			r = (r+1) & CMX_BUFF_MASK;
+		}
+		while (r != rr) {
+			sample = *c++;
+			if (sample < -32768)
+				sample = -32768;
+			else if (sample > 32767)
+				sample = 32767;
+			*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+			    /* conf(echo) */
+			r = (r+1) & CMX_BUFF_MASK;
+		}
+	}
+	dsp->tx_R = t;
+	goto send_packet;
+
+send_packet:
+	/*
+	 * send tx-data if enabled - don't filter,
+	 * becuase we want what we send, not what we filtered
+	 */
+	if (dsp->tx_data) {
+		/* PREPARE RESULT */
+		txskb = mI_alloc_skb(len, GFP_ATOMIC);
+		if (!txskb) {
+			printk(KERN_ERR
+			    "FATAL ERROR in mISDN_dsp.o: "
+			    "cannot alloc %d bytes\n", len);
+		} else {
+			thh = mISDN_HEAD_P(txskb);
+			thh->prim = DL_DATA_REQ;
+			thh->id = 0;
+			memcpy(skb_put(txskb, len), nskb->data+preload, len);
+			/* queue (trigger later) */
+			skb_queue_tail(&dsp->sendq, txskb);
+		}
+	}
+	/* adjust volume */
+	if (dsp->tx_volume)
+		dsp_change_volume(nskb, dsp->tx_volume);
+	/* pipeline */
+	if (dsp->pipeline.inuse)
+		dsp_pipeline_process_tx(&dsp->pipeline, nskb->data, nskb->len);
+	/* crypt */
+	if (dsp->bf_enable)
+		dsp_bf_encrypt(dsp, nskb->data, nskb->len);
+	/* queue and trigger */
+	skb_queue_tail(&dsp->sendq, nskb);
+	schedule_work(&dsp->workq);
+}
+
+u32	samplecount;
+struct timer_list dsp_spl_tl;
+u32	dsp_spl_jiffies; /* calculate the next time to fire */
+u32	dsp_start_jiffies; /* jiffies at the time, the calculation begins */
+struct timeval dsp_start_tv; /* time at start of calculation */
+
+void
+dsp_cmx_send(void *arg)
+{
+	struct dsp_conf *conf;
+	struct dsp_conf_member *member;
+	struct dsp *dsp;
+	int mustmix, members;
+	s32 mixbuffer[MAX_POLL+100], *c;
+	u8 *p, *q;
+	int r, rr;
+	int jittercheck = 0, delay, i;
+	u_long flags;
+	struct timeval tv;
+	u32 elapsed;
+	s16 length;
+
+	/* lock */
+	spin_lock_irqsave(&dsp_lock, flags);
+
+	if (!dsp_start_tv.tv_sec) {
+		do_gettimeofday(&dsp_start_tv);
+		length = dsp_poll;
+	} else {
+		do_gettimeofday(&tv);
+		elapsed = ((tv.tv_sec - dsp_start_tv.tv_sec) * 8000)
+		    + ((s32)(tv.tv_usec / 125) - (dsp_start_tv.tv_usec / 125));
+		dsp_start_tv.tv_sec = tv.tv_sec;
+		dsp_start_tv.tv_usec = tv.tv_usec;
+		length = elapsed;
+	}
+	if (length > MAX_POLL + 100)
+		length = MAX_POLL + 100;
+/* printk(KERN_DEBUG "len=%d dsp_count=0x%x.%04x dsp_poll_diff=0x%x.%04x\n",
+ length, dsp_count >> 16, dsp_count & 0xffff, dsp_poll_diff >> 16,
+ dsp_poll_diff & 0xffff);
+ */
+
+	/*
+	 * check if jitter needs to be checked
+	 * (this is about every second = 8192 samples)
+	 */
+	samplecount += length;
+	if ((samplecount & 8191) < length)
+		jittercheck = 1;
+
+	/* loop all members that do not require conference mixing */
+	list_for_each_entry(dsp, &dsp_ilist, list) {
+		if (dsp->hdlc)
+			continue;
+		conf = dsp->conf;
+		mustmix = 0;
+		members = 0;
+		if (conf) {
+			members = count_list_member(&conf->mlist);
+#ifdef CMX_CONF_DEBUG
+			if (conf->software && members > 1)
+#else
+			if (conf->software && members > 2)
+#endif
+				mustmix = 1;
+		}
+
+		/* transmission required */
+		if (!mustmix) {
+			dsp_cmx_send_member(dsp, length, mixbuffer, members);
+
+			/*
+			 * unused mixbuffer is given to prevent a
+			 * potential null-pointer-bug
+			 */
+		}
+	}
+
+	/* loop all members that require conference mixing */
+	list_for_each_entry(conf, &conf_ilist, list) {
+		/* count members and check hardware */
+		members = count_list_member(&conf->mlist);
+#ifdef CMX_CONF_DEBUG
+		if (conf->software && members > 1) {
+#else
+		if (conf->software && members > 2) {
+#endif
+			/* check for hdlc conf */
+			member = list_entry(conf->mlist.next,
+				struct dsp_conf_member, list);
+			if (member->dsp->hdlc)
+				continue;
+			/* mix all data */
+			memset(mixbuffer, 0, length*sizeof(s32));
+			list_for_each_entry(member, &conf->mlist, list) {
+				dsp = member->dsp;
+				/* get range of data to mix */
+				c = mixbuffer;
+				q = dsp->rx_buff;
+				r = dsp->rx_R;
+				rr = (r + length) & CMX_BUFF_MASK;
+				/* add member's data */
+				while (r != rr) {
+					*c++ += dsp_audio_law_to_s32[q[r]];
+					r = (r+1) & CMX_BUFF_MASK;
+				}
+			}
+
+			/* process each member */
+			list_for_each_entry(member, &conf->mlist, list) {
+				/* transmission */
+				dsp_cmx_send_member(member->dsp, length,
+				    mixbuffer, members);
+			}
+		}
+	}
+
+	/* delete rx-data, increment buffers, change pointers */
+	list_for_each_entry(dsp, &dsp_ilist, list) {
+		if (dsp->hdlc)
+			continue;
+		p = dsp->rx_buff;
+		q = dsp->tx_buff;
+		r = dsp->rx_R;
+		/* move receive pointer when receiving */
+		if (!dsp->rx_is_off) {
+			rr = (r + length) & CMX_BUFF_MASK;
+			/* delete rx-data */
+			while (r != rr) {
+				p[r] = dsp_silence;
+				r = (r+1) & CMX_BUFF_MASK;
+			}
+			/* increment rx-buffer pointer */
+			dsp->rx_R = r; /* write incremented read pointer */
+		}
+
+		/* check current rx_delay */
+		delay = (dsp->rx_W-dsp->rx_R) & CMX_BUFF_MASK;
+		if (delay >= CMX_BUFF_HALF)
+			delay = 0; /* will be the delay before next write */
+		/* check for lower delay */
+		if (delay < dsp->rx_delay[0])
+			dsp->rx_delay[0] = delay;
+		/* check current tx_delay */
+		delay = (dsp->tx_W-dsp->tx_R) & CMX_BUFF_MASK;
+		if (delay >= CMX_BUFF_HALF)
+			delay = 0; /* will be the delay before next write */
+		/* check for lower delay */
+		if (delay < dsp->tx_delay[0])
+			dsp->tx_delay[0] = delay;
+		if (jittercheck) {
+			/* find the lowest of all rx_delays */
+			delay = dsp->rx_delay[0];
+			i = 1;
+			while (i < MAX_SECONDS_JITTER_CHECK) {
+				if (delay > dsp->rx_delay[i])
+					delay = dsp->rx_delay[i];
+				i++;
+			}
+			/*
+			 * remove rx_delay only if we have delay AND we
+			 * have not preset cmx_delay
+			 */
+			if (delay && !dsp->cmx_delay) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s lowest rx_delay of %d bytes for"
+					    " dsp %s are now removed.\n",
+					    __func__, delay,
+					    dsp->name);
+				r = dsp->rx_R;
+				rr = (r + delay) & CMX_BUFF_MASK;
+				/* delete rx-data */
+				while (r != rr) {
+					p[r] = dsp_silence;
+					r = (r+1) & CMX_BUFF_MASK;
+				}
+				/* increment rx-buffer pointer */
+				dsp->rx_R = r;
+				    /* write incremented read pointer */
+			}
+			/* find the lowest of all tx_delays */
+			delay = dsp->tx_delay[0];
+			i = 1;
+			while (i < MAX_SECONDS_JITTER_CHECK) {
+				if (delay > dsp->tx_delay[i])
+					delay = dsp->tx_delay[i];
+				i++;
+			}
+			/*
+			 * remove delay only if we have delay AND we
+			 * have enabled tx_dejitter
+			 */
+			if (delay && dsp->tx_dejitter) {
+				if (dsp_debug & DEBUG_DSP_CMX)
+					printk(KERN_DEBUG
+					    "%s lowest tx_delay of %d bytes for"
+					    " dsp %s are now removed.\n",
+					    __func__, delay,
+					    dsp->name);
+				r = dsp->tx_R;
+				rr = (r + delay) & CMX_BUFF_MASK;
+				/* delete tx-data */
+				while (r != rr) {
+					q[r] = dsp_silence;
+					r = (r+1) & CMX_BUFF_MASK;
+				}
+				/* increment rx-buffer pointer */
+				dsp->tx_R = r;
+				    /* write incremented read pointer */
+			}
+			/* scroll up delays */
+			i = MAX_SECONDS_JITTER_CHECK - 1;
+			while (i) {
+				dsp->rx_delay[i] = dsp->rx_delay[i-1];
+				dsp->tx_delay[i] = dsp->tx_delay[i-1];
+				i--;
+			}
+			dsp->tx_delay[0] = CMX_BUFF_HALF; /* (infinite) delay */
+			dsp->rx_delay[0] = CMX_BUFF_HALF; /* (infinite) delay */
+		}
+	}
+
+	/* if next event would be in the past ... */
+	if ((s32)(dsp_spl_jiffies+dsp_tics-jiffies) <= 0)
+		dsp_spl_jiffies = jiffies + 1;
+	else
+		dsp_spl_jiffies += dsp_tics;
+
+	dsp_spl_tl.expires = dsp_spl_jiffies;
+	add_timer(&dsp_spl_tl);
+
+	/* unlock */
+	spin_unlock_irqrestore(&dsp_lock, flags);
+}
+
+/*
+ * audio data is transmitted from upper layer to the dsp
+ */
+void
+dsp_cmx_transmit(struct dsp *dsp, struct sk_buff *skb)
+{
+	u_int w, ww;
+	u8 *d, *p;
+	int space; /* todo: , l = skb->len; */
+#ifdef CMX_TX_DEBUG
+	char debugbuf[256] = "";
+#endif
+
+	/* check if there is enough space, and then copy */
+	w = dsp->tx_W;
+	ww = dsp->tx_R;
+	p = dsp->tx_buff;
+	d = skb->data;
+	space = ww-w;
+	if (space <= 0)
+		space += CMX_BUFF_SIZE;
+	/* write-pointer should not overrun nor reach read pointer */
+	if (space-1 < skb->len)
+		/* write to the space we have left */
+		ww = (ww - 1) & CMX_BUFF_MASK;
+	else
+		/* write until all byte are copied */
+		ww = (w + skb->len) & CMX_BUFF_MASK;
+	dsp->tx_W = ww;
+
+	/* show current buffer */
+#ifdef CMX_DEBUG
+	printk(KERN_DEBUG
+	    "cmx_transmit(dsp=%lx) %d bytes to 0x%x-0x%x. %s\n",
+	    (u_long)dsp, (ww-w)&CMX_BUFF_MASK, w, ww, dsp->name);
+#endif
+
+	/* copy transmit data to tx-buffer */
+#ifdef CMX_TX_DEBUG
+	sprintf(debugbuf, "TX getting (%04x-%04x)%p: ", w, ww, p);
+#endif
+	while (w != ww) {
+#ifdef CMX_TX_DEBUG
+		if (strlen(debugbuf) < 48)
+			sprintf(debugbuf+strlen(debugbuf), " %02x", *d);
+#endif
+		p[w] = *d++;
+		w = (w+1) & CMX_BUFF_MASK;
+	}
+#ifdef CMX_TX_DEBUG
+	printk(KERN_DEBUG "%s\n", debugbuf);
+#endif
+
+}
+
+/*
+ * hdlc data is received from card and sent to all members.
+ */
+void
+dsp_cmx_hdlc(struct dsp *dsp, struct sk_buff *skb)
+{
+	struct sk_buff *nskb = NULL;
+	struct dsp_conf_member *member;
+	struct mISDNhead *hh;
+
+	/* not if not active */
+	if (!dsp->b_active)
+		return;
+
+	/* check if we have sompen */
+	if (skb->len < 1)
+		return;
+
+	/* no conf */
+	if (!dsp->conf) {
+		/* in case of hardware (echo) */
+		if (dsp->pcm_slot_tx >= 0)
+			return;
+		if (dsp->echo)
+			nskb = skb_clone(skb, GFP_ATOMIC);
+			if (nskb) {
+				hh = mISDN_HEAD_P(nskb);
+				hh->prim = PH_DATA_REQ;
+				hh->id = 0;
+				skb_queue_tail(&dsp->sendq, nskb);
+				schedule_work(&dsp->workq);
+			}
+		return;
+	}
+	/* in case of hardware conference */
+	if (dsp->conf->hardware)
+		return;
+	list_for_each_entry(member, &dsp->conf->mlist, list) {
+		if (dsp->echo || member->dsp != dsp) {
+			nskb = skb_clone(skb, GFP_ATOMIC);
+			if (nskb) {
+				hh = mISDN_HEAD_P(nskb);
+				hh->prim = PH_DATA_REQ;
+				hh->id = 0;
+				skb_queue_tail(&member->dsp->sendq, nskb);
+				schedule_work(&member->dsp->workq);
+			}
+		}
+	}
+}
+
+
diff --git a/drivers/isdn/mISDN/dsp_core.c b/drivers/isdn/mISDN/dsp_core.c
new file mode 100644
index 00000000000..2f10ed82c0d
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_core.c
@@ -0,0 +1,1191 @@
+/*
+ * Author       Andreas Eversberg (jolly@eversberg.eu)
+ * Based on source code structure by
+ *		Karsten Keil (keil@isdn4linux.de)
+ *
+ *		This file is (c) under GNU PUBLIC LICENSE
+ *		For changes and modifications please read
+ *		../../../Documentation/isdn/mISDN.cert
+ *
+ * Thanks to    Karsten Keil (great drivers)
+ *              Cologne Chip (great chips)
+ *
+ * This module does:
+ *		Real-time tone generation
+ *		DTMF detection
+ *		Real-time cross-connection and conferrence
+ *		Compensate jitter due to system load and hardware fault.
+ *		All features are done in kernel space and will be realized
+ *		using hardware, if available and supported by chip set.
+ *		Blowfish encryption/decryption
+ */
+
+/* STRUCTURE:
+ *
+ * The dsp module provides layer 2 for b-channels (64kbit). It provides
+ * transparent audio forwarding with special digital signal processing:
+ *
+ * - (1) generation of tones
+ * - (2) detection of dtmf tones
+ * - (3) crossconnecting and conferences (clocking)
+ * - (4) echo generation for delay test
+ * - (5) volume control
+ * - (6) disable receive data
+ * - (7) pipeline
+ * - (8) encryption/decryption
+ *
+ * Look:
+ *             TX            RX
+ *         ------upper layer------
+ *             |             ^
+ *             |             |(6)
+ *             v             |
+ *       +-----+-------------+-----+
+ *       |(3)(4)                   |
+ *       |           CMX           |
+ *       |                         |
+ *       |           +-------------+
+ *       |           |       ^
+ *       |           |       |
+ *       |+---------+|  +----+----+
+ *       ||(1)      ||  |(2)      |
+ *       ||         ||  |         |
+ *       ||  Tones  ||  |  DTMF   |
+ *       ||         ||  |         |
+ *       ||         ||  |         |
+ *       |+----+----+|  +----+----+
+ *       +-----+-----+       ^
+ *             |             |
+ *             v             |
+ *        +----+----+   +----+----+
+ *        |(5)      |   |(5)      |
+ *        |         |   |         |
+ *        |TX Volume|   |RX Volume|
+ *        |         |   |         |
+ *        |         |   |         |
+ *        +----+----+   +----+----+
+ *             |             ^
+ *             |             |
+ *             v             |
+ *        +----+-------------+----+
+ *        |(7)                    |
+ *        |                       |
+ *        |  Pipeline Processing  |
+ *        |                       |
+ *        |                       |
+ *        +----+-------------+----+
+ *             |             ^
+ *             |             |
+ *             v             |
+ *        +----+----+   +----+----+
+ *        |(8)      |   |(8)      |
+ *        |         |   |         |
+ *        | Encrypt |   | Decrypt |
+ *        |         |   |         |
+ *        |         |   |         |
+ *        +----+----+   +----+----+
+ *             |             ^
+ *             |             |
+ *             v             |
+ *         ------card  layer------
+ *             TX            RX
+ *
+ * Above you can see the logical data flow. If software is used to do the
+ * process, it is actually the real data flow. If hardware is used, data
+ * may not flow, but hardware commands to the card, to provide the data flow
+ * as shown.
+ *
+ * NOTE: The channel must be activated in order to make dsp work, even if
+ * no data flow to the upper layer is intended. Activation can be done
+ * after and before controlling the setting using PH_CONTROL requests.
+ *
+ * DTMF: Will be detected by hardware if possible. It is done before CMX
+ * processing.
+ *
+ * Tones: Will be generated via software if endless looped audio fifos are
+ * not supported by hardware. Tones will override all data from CMX.
+ * It is not required to join a conference to use tones at any time.
+ *
+ * CMX: Is transparent when not used. When it is used, it will do
+ * crossconnections and conferences via software if not possible through
+ * hardware. If hardware capability is available, hardware is used.
+ *
+ * Echo: Is generated by CMX and is used to check performane of hard and
+ * software CMX.
+ *
+ * The CMX has special functions for conferences with one, two and more
+ * members. It will allow different types of data flow. Receive and transmit
+ * data to/form upper layer may be swithed on/off individually without loosing
+ * features of CMX, Tones and DTMF.
+ *
+ * Echo Cancellation: Sometimes we like to cancel echo from the interface.
+ * Note that a VoIP call may not have echo caused by the IP phone. The echo
+ * is generated by the telephone line connected to it. Because the delay
+ * is high, it becomes an echo. RESULT: Echo Cachelation is required if
+ * both echo AND delay is applied to an interface.
+ * Remember that software CMX always generates a more or less delay.
+ *
+ * If all used features can be realized in hardware, and if transmit and/or
+ * receive data ist disabled, the card may not send/receive any data at all.
+ * Not receiving is usefull if only announcements are played. Not sending is
+ * usefull if an answering machine records audio. Not sending and receiving is
+ * usefull during most states of the call. If supported by hardware, tones
+ * will be played without cpu load. Small PBXs and NT-Mode applications will
+ * not need expensive hardware when processing calls.
+ *
+ *
+ * LOCKING:
+ *
+ * When data is received from upper or lower layer (card), the complete dsp
+ * module is locked by a global lock.  This lock MUST lock irq, because it
+ * must lock timer events by DSP poll timer.
+ * When data is ready to be transmitted down, the data is queued and sent
+ * outside lock and timer event.
+ * PH_CONTROL must not change any settings, join or split conference members
+ * during process of data.
+ *
+ * HDLC:
+ *
+ * It works quite the same as transparent, except that HDLC data is forwarded
+ * to all other conference members if no hardware bridging is possible.
+ * Send data will be writte to sendq. Sendq will be sent if confirm is received.
+ * Conference cannot join, if one member is not hdlc.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include "core.h"
+#include "dsp.h"
+
+const char *mISDN_dsp_revision = "2.0";
+
+static int debug;
+static int options;
+static int poll;
+static int dtmfthreshold = 100;
+
+MODULE_AUTHOR("Andreas Eversberg");
+module_param(debug, uint, S_IRUGO | S_IWUSR);
+module_param(options, uint, S_IRUGO | S_IWUSR);
+module_param(poll, uint, S_IRUGO | S_IWUSR);
+module_param(dtmfthreshold, uint, S_IRUGO | S_IWUSR);
+MODULE_LICENSE("GPL");
+
+/*int spinnest = 0;*/
+
+spinlock_t dsp_lock; /* global dsp lock */
+struct list_head dsp_ilist;
+struct list_head conf_ilist;
+int dsp_debug;
+int dsp_options;
+int dsp_poll, dsp_tics;
+
+/* check if rx may be turned off or must be turned on */
+static void
+dsp_rx_off_member(struct dsp *dsp)
+{
+	struct mISDN_ctrl_req	cq;
+	int rx_off = 1;
+
+	if (!dsp->features_rx_off)
+		return;
+
+	/* not disabled */
+	if (!dsp->rx_disabled)
+		rx_off = 0;
+	/* software dtmf */
+	else if (dsp->dtmf.software)
+		rx_off = 0;
+	/* echo in software */
+	else if (dsp->echo && dsp->pcm_slot_tx < 0)
+		rx_off = 0;
+	/* bridge in software */
+	else if (dsp->conf) {
+		if (dsp->conf->software)
+			rx_off = 0;
+	}
+
+	if (rx_off == dsp->rx_is_off)
+		return;
+
+	if (!dsp->ch.peer) {
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: no peer, no rx_off\n",
+				__func__);
+		return;
+	}
+	cq.op = MISDN_CTRL_RX_OFF;
+	cq.p1 = rx_off;
+	if (dsp->ch.peer->ctrl(dsp->ch.peer, CONTROL_CHANNEL, &cq)) {
+		printk(KERN_DEBUG "%s: 2nd CONTROL_CHANNEL failed\n",
+			__func__);
+		return;
+	}
+	dsp->rx_is_off = rx_off;
+	if (dsp_debug & DEBUG_DSP_CORE)
+		printk(KERN_DEBUG "%s: %s set rx_off = %d\n",
+			__func__, dsp->name, rx_off);
+}
+static void
+dsp_rx_off(struct dsp *dsp)
+{
+	struct dsp_conf_member	*member;
+
+	if (dsp_options & DSP_OPT_NOHARDWARE)
+		return;
+
+	/* no conf */
+	if (!dsp->conf) {
+		dsp_rx_off_member(dsp);
+		return;
+	}
+	/* check all members in conf */
+	list_for_each_entry(member, &dsp->conf->mlist, list) {
+		dsp_rx_off_member(member->dsp);
+	}
+}
+
+static int
+dsp_control_req(struct dsp *dsp, struct mISDNhead *hh, struct sk_buff *skb)
+{
+	struct		sk_buff *nskb;
+	int ret = 0;
+	int cont;
+	u8 *data;
+	int len;
+
+	if (skb->len < sizeof(int))
+		printk(KERN_ERR "%s: PH_CONTROL message too short\n", __func__);
+	cont = *((int *)skb->data);
+	len = skb->len - sizeof(int);
+	data = skb->data + sizeof(int);
+
+	switch (cont) {
+	case DTMF_TONE_START: /* turn on DTMF */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: start dtmf\n", __func__);
+		if (len == sizeof(int)) {
+			printk(KERN_NOTICE "changing DTMF Threshold "
+				"to %d\n", *((int *)data));
+			dsp->dtmf.treshold = (*(int *)data) * 10000;
+		}
+		/* init goertzel */
+		dsp_dtmf_goertzel_init(dsp);
+
+		/* check dtmf hardware */
+		dsp_dtmf_hardware(dsp);
+		break;
+	case DTMF_TONE_STOP: /* turn off DTMF */
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: stop dtmf\n", __func__);
+		dsp->dtmf.hardware = 0;
+		dsp->dtmf.software = 0;
+		break;
+	case DSP_CONF_JOIN: /* join / update conference */
+		if (len < sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		if (*((u32 *)data) == 0)
+			goto conf_split;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: join conference %d\n",
+				__func__, *((u32 *)data));
+		ret = dsp_cmx_conf(dsp, *((u32 *)data));
+			/* dsp_cmx_hardware will also be called here */
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_CONF_SPLIT: /* remove from conference */
+conf_split:
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: release conference\n", __func__);
+		ret = dsp_cmx_conf(dsp, 0);
+			/* dsp_cmx_hardware will also be called here */
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		dsp_rx_off(dsp);
+		break;
+	case DSP_TONE_PATT_ON: /* play tone */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len < sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: turn tone 0x%x on\n",
+				__func__, *((int *)skb->data));
+		ret = dsp_tone(dsp, *((int *)data));
+		if (!ret) {
+			dsp_cmx_hardware(dsp->conf, dsp);
+			dsp_rx_off(dsp);
+		}
+		if (!dsp->tone.tone)
+			goto tone_off;
+		break;
+	case DSP_TONE_PATT_OFF: /* stop tone */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: turn tone off\n", __func__);
+		dsp_tone(dsp, 0);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		/* reset tx buffers (user space data) */
+tone_off:
+		dsp->rx_W = 0;
+		dsp->rx_R = 0;
+		break;
+	case DSP_VOL_CHANGE_TX: /* change volume */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len < sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->tx_volume = *((int *)data);
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: change tx vol to %d\n",
+				__func__, dsp->tx_volume);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_dtmf_hardware(dsp);
+		dsp_rx_off(dsp);
+		break;
+	case DSP_VOL_CHANGE_RX: /* change volume */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len < sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->rx_volume = *((int *)data);
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: change rx vol to %d\n",
+				__func__, dsp->tx_volume);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_dtmf_hardware(dsp);
+		dsp_rx_off(dsp);
+		break;
+	case DSP_ECHO_ON: /* enable echo */
+		dsp->echo = 1; /* soft echo */
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: enable cmx-echo\n", __func__);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_ECHO_OFF: /* disable echo */
+		dsp->echo = 0;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: disable cmx-echo\n", __func__);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_RECEIVE_ON: /* enable receive to user space */
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: enable receive to user "
+				"space\n", __func__);
+		dsp->rx_disabled = 0;
+		dsp_rx_off(dsp);
+		break;
+	case DSP_RECEIVE_OFF: /* disable receive to user space */
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: disable receive to "
+				"user space\n", __func__);
+		dsp->rx_disabled = 1;
+		dsp_rx_off(dsp);
+		break;
+	case DSP_MIX_ON: /* enable mixing of tx data */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: enable mixing of "
+				"tx-data with conf mebers\n", __func__);
+		dsp->tx_mix = 1;
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_MIX_OFF: /* disable mixing of tx data */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: disable mixing of "
+				"tx-data with conf mebers\n", __func__);
+		dsp->tx_mix = 0;
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_TXDATA_ON: /* enable txdata */
+		dsp->tx_data = 1;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: enable tx-data\n", __func__);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_TXDATA_OFF: /* disable txdata */
+		dsp->tx_data = 0;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: disable tx-data\n", __func__);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		if (dsp_debug & DEBUG_DSP_CMX)
+			dsp_cmx_debug(dsp);
+		break;
+	case DSP_DELAY: /* use delay algorithm instead of dynamic
+			   jitter algorithm */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len < sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->cmx_delay = (*((int *)data)) << 3;
+			/* miliseconds to samples */
+		if (dsp->cmx_delay >= (CMX_BUFF_HALF>>1))
+			/* clip to half of maximum usable buffer
+			(half of half buffer) */
+			dsp->cmx_delay = (CMX_BUFF_HALF>>1) - 1;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: use delay algorithm to "
+				"compensate jitter (%d samples)\n",
+				__func__, dsp->cmx_delay);
+		break;
+	case DSP_JITTER: /* use dynamic jitter algorithm instead of
+		    delay algorithm */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->cmx_delay = 0;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: use jitter algorithm to "
+				"compensate jitter\n", __func__);
+		break;
+	case DSP_TX_DEJITTER: /* use dynamic jitter algorithm for tx-buffer */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->tx_dejitter = 1;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: use dejitter on TX "
+				"buffer\n", __func__);
+		break;
+	case DSP_TX_DEJ_OFF: /* use tx-buffer without dejittering*/
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		dsp->tx_dejitter = 0;
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: use TX buffer without "
+				"dejittering\n", __func__);
+		break;
+	case DSP_PIPELINE_CFG:
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len > 0 && ((char *)data)[len - 1]) {
+			printk(KERN_DEBUG "%s: pipeline config string "
+				"is not NULL terminated!\n", __func__);
+			ret = -EINVAL;
+		} else {
+			dsp->pipeline.inuse = 1;
+			dsp_cmx_hardware(dsp->conf, dsp);
+			ret = dsp_pipeline_build(&dsp->pipeline,
+				len > 0 ? (char *)data : NULL);
+			dsp_cmx_hardware(dsp->conf, dsp);
+			dsp_rx_off(dsp);
+		}
+		break;
+	case DSP_BF_ENABLE_KEY: /* turn blowfish on */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (len < 4 || len > 56) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: turn blowfish on (key "
+				"not shown)\n", __func__);
+		ret = dsp_bf_init(dsp, (u8 *)data, len);
+		/* set new cont */
+		if (!ret)
+			cont = DSP_BF_ACCEPT;
+		else
+			cont = DSP_BF_REJECT;
+		/* send indication if it worked to set it */
+		nskb = _alloc_mISDN_skb(PH_CONTROL_IND, MISDN_ID_ANY,
+			sizeof(int), &cont, GFP_ATOMIC);
+		if (nskb) {
+			if (dsp->up) {
+				if (dsp->up->send(dsp->up, nskb))
+					dev_kfree_skb(nskb);
+			} else
+				dev_kfree_skb(nskb);
+		}
+		if (!ret) {
+			dsp_cmx_hardware(dsp->conf, dsp);
+			dsp_dtmf_hardware(dsp);
+			dsp_rx_off(dsp);
+		}
+		break;
+	case DSP_BF_DISABLE: /* turn blowfish off */
+		if (dsp->hdlc) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: turn blowfish off\n", __func__);
+		dsp_bf_cleanup(dsp);
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_dtmf_hardware(dsp);
+		dsp_rx_off(dsp);
+		break;
+	default:
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: ctrl req %x unhandled\n",
+				__func__, cont);
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static void
+get_features(struct mISDNchannel *ch)
+{
+	struct dsp		*dsp = container_of(ch, struct dsp, ch);
+	struct mISDN_ctrl_req	cq;
+
+	if (dsp_options & DSP_OPT_NOHARDWARE)
+		return;
+	if (!ch->peer) {
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: no peer, no features\n",
+				__func__);
+		return;
+	}
+	memset(&cq, 0, sizeof(cq));
+	cq.op = MISDN_CTRL_GETOP;
+	if (ch->peer->ctrl(ch->peer, CONTROL_CHANNEL, &cq) < 0) {
+		printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n",
+			__func__);
+		return;
+	}
+	if (cq.op & MISDN_CTRL_RX_OFF)
+		dsp->features_rx_off = 1;
+	if ((cq.op & MISDN_CTRL_HW_FEATURES_OP)) {
+		cq.op = MISDN_CTRL_HW_FEATURES;
+		*((u_long *)&cq.p1) = (u_long)&dsp->features;
+		if (ch->peer->ctrl(ch->peer, CONTROL_CHANNEL, &cq)) {
+			printk(KERN_DEBUG "%s: 2nd CONTROL_CHANNEL failed\n",
+				__func__);
+		}
+	} else
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: features not supported for %s\n",
+				__func__, dsp->name);
+}
+
+static int
+dsp_function(struct mISDNchannel *ch,  struct sk_buff *skb)
+{
+	struct dsp			*dsp = container_of(ch, struct dsp, ch);
+	struct mISDNhead	*hh;
+	int			ret = 0;
+	u8			*digits;
+	int			cont;
+	struct			sk_buff *nskb;
+	u_long			flags;
+
+	hh = mISDN_HEAD_P(skb);
+	switch (hh->prim) {
+	/* FROM DOWN */
+	case (PH_DATA_CNF):
+		dsp->data_pending = 0;
+		/* trigger next hdlc frame, if any */
+		if (dsp->hdlc) {
+			spin_lock_irqsave(&dsp_lock, flags);
+			if (dsp->b_active)
+				schedule_work(&dsp->workq);
+			spin_unlock_irqrestore(&dsp_lock, flags);
+		}
+		break;
+	case (PH_DATA_IND):
+	case (DL_DATA_IND):
+		if (skb->len < 1) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp->rx_is_off) {
+			if (dsp_debug & DEBUG_DSP_CORE)
+				printk(KERN_DEBUG "%s: rx-data during rx_off"
+					" for %s\n",
+				__func__, dsp->name);
+		}
+		if (dsp->hdlc) {
+			/* hdlc */
+			spin_lock_irqsave(&dsp_lock, flags);
+			dsp_cmx_hdlc(dsp, skb);
+			spin_unlock_irqrestore(&dsp_lock, flags);
+			if (dsp->rx_disabled) {
+				/* if receive is not allowed */
+				break;
+			}
+			hh->prim = DL_DATA_IND;
+			if (dsp->up)
+				return dsp->up->send(dsp->up, skb);
+			break;
+		}
+
+		/* decrypt if enabled */
+		if (dsp->bf_enable)
+			dsp_bf_decrypt(dsp, skb->data, skb->len);
+		/* pipeline */
+		if (dsp->pipeline.inuse)
+			dsp_pipeline_process_rx(&dsp->pipeline, skb->data,
+				skb->len);
+		/* change volume if requested */
+		if (dsp->rx_volume)
+			dsp_change_volume(skb, dsp->rx_volume);
+
+		/* check if dtmf soft decoding is turned on */
+		if (dsp->dtmf.software) {
+			digits = dsp_dtmf_goertzel_decode(dsp, skb->data,
+				skb->len, (dsp_options&DSP_OPT_ULAW)?1:0);
+			while (*digits) {
+				if (dsp_debug & DEBUG_DSP_DTMF)
+					printk(KERN_DEBUG "%s: digit"
+					    "(%c) to layer %s\n",
+					    __func__, *digits, dsp->name);
+				cont = DTMF_TONE_VAL | *digits;
+				nskb = _alloc_mISDN_skb(PH_CONTROL_IND,
+				    MISDN_ID_ANY, sizeof(int), &cont,
+				    GFP_ATOMIC);
+				if (nskb) {
+					if (dsp->up) {
+						if (dsp->up->send(
+						    dsp->up, nskb))
+						dev_kfree_skb(nskb);
+					} else
+						dev_kfree_skb(nskb);
+				}
+				digits++;
+			}
+		}
+		/* we need to process receive data if software */
+		spin_lock_irqsave(&dsp_lock, flags);
+		if (dsp->pcm_slot_tx < 0 && dsp->pcm_slot_rx < 0) {
+			/* process data from card at cmx */
+			dsp_cmx_receive(dsp, skb);
+		}
+		spin_unlock_irqrestore(&dsp_lock, flags);
+
+		if (dsp->rx_disabled) {
+			/* if receive is not allowed */
+			break;
+		}
+		hh->prim = DL_DATA_IND;
+		if (dsp->up)
+			return dsp->up->send(dsp->up, skb);
+		break;
+	case (PH_CONTROL_IND):
+		if (dsp_debug & DEBUG_DSP_DTMFCOEFF)
+			printk(KERN_DEBUG "%s: PH_CONTROL INDICATION "
+				"received: %x (len %d) %s\n", __func__,
+				hh->id, skb->len, dsp->name);
+		switch (hh->id) {
+		case (DTMF_HFC_COEF): /* getting coefficients */
+			if (!dsp->dtmf.hardware) {
+				if (dsp_debug & DEBUG_DSP_DTMFCOEFF)
+					printk(KERN_DEBUG "%s: ignoring DTMF "
+						"coefficients from HFC\n",
+						__func__);
+				break;
+			}
+			digits = dsp_dtmf_goertzel_decode(dsp, skb->data,
+				skb->len, 2);
+			while (*digits) {
+				int k;
+				struct sk_buff *nskb;
+				if (dsp_debug & DEBUG_DSP_DTMF)
+					printk(KERN_DEBUG "%s: digit"
+					    "(%c) to layer %s\n",
+					    __func__, *digits, dsp->name);
+				k = *digits | DTMF_TONE_VAL;
+				nskb = _alloc_mISDN_skb(PH_CONTROL_IND,
+					MISDN_ID_ANY, sizeof(int), &k,
+					GFP_ATOMIC);
+				if (nskb) {
+					if (dsp->up) {
+						if (dsp->up->send(
+						    dsp->up, nskb))
+						dev_kfree_skb(nskb);
+					} else
+						dev_kfree_skb(nskb);
+				}
+				digits++;
+			}
+			break;
+		case (HFC_VOL_CHANGE_TX): /* change volume */
+			if (skb->len != sizeof(int)) {
+				ret = -EINVAL;
+				break;
+			}
+			spin_lock_irqsave(&dsp_lock, flags);
+			dsp->tx_volume = *((int *)skb->data);
+			if (dsp_debug & DEBUG_DSP_CORE)
+				printk(KERN_DEBUG "%s: change tx volume to "
+					"%d\n", __func__, dsp->tx_volume);
+			dsp_cmx_hardware(dsp->conf, dsp);
+			dsp_dtmf_hardware(dsp);
+			dsp_rx_off(dsp);
+			spin_unlock_irqrestore(&dsp_lock, flags);
+			break;
+		default:
+			if (dsp_debug & DEBUG_DSP_CORE)
+				printk(KERN_DEBUG "%s: ctrl ind %x unhandled "
+					"%s\n", __func__, hh->id, dsp->name);
+			ret = -EINVAL;
+		}
+		break;
+	case (PH_ACTIVATE_IND):
+	case (PH_ACTIVATE_CNF):
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: b_channel is now active %s\n",
+				__func__, dsp->name);
+		/* bchannel now active */
+		spin_lock_irqsave(&dsp_lock, flags);
+		dsp->b_active = 1;
+		dsp->data_pending = 0;
+		dsp->rx_init = 1;
+			/* rx_W and rx_R will be adjusted on first frame */
+		dsp->rx_W = 0;
+		dsp->rx_R = 0;
+		memset(dsp->rx_buff, 0, sizeof(dsp->rx_buff));
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_dtmf_hardware(dsp);
+		dsp_rx_off(dsp);
+		spin_unlock_irqrestore(&dsp_lock, flags);
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: done with activation, sending "
+				"confirm to user space. %s\n", __func__,
+				dsp->name);
+		/* send activation to upper layer */
+		hh->prim = DL_ESTABLISH_CNF;
+		if (dsp->up)
+			return dsp->up->send(dsp->up, skb);
+		break;
+	case (PH_DEACTIVATE_IND):
+	case (PH_DEACTIVATE_CNF):
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: b_channel is now inactive %s\n",
+				__func__, dsp->name);
+		/* bchannel now inactive */
+		spin_lock_irqsave(&dsp_lock, flags);
+		dsp->b_active = 0;
+		dsp->data_pending = 0;
+		dsp_cmx_hardware(dsp->conf, dsp);
+		dsp_rx_off(dsp);
+		spin_unlock_irqrestore(&dsp_lock, flags);
+		hh->prim = DL_RELEASE_CNF;
+		if (dsp->up)
+			return dsp->up->send(dsp->up, skb);
+		break;
+	/* FROM UP */
+	case (DL_DATA_REQ):
+	case (PH_DATA_REQ):
+		if (skb->len < 1) {
+			ret = -EINVAL;
+			break;
+		}
+		if (dsp->hdlc) {
+			/* hdlc */
+			spin_lock_irqsave(&dsp_lock, flags);
+			if (dsp->b_active) {
+				skb_queue_tail(&dsp->sendq, skb);
+				schedule_work(&dsp->workq);
+			}
+			spin_unlock_irqrestore(&dsp_lock, flags);
+			return 0;
+		}
+		/* send data to tx-buffer (if no tone is played) */
+		if (!dsp->tone.tone) {
+			spin_lock_irqsave(&dsp_lock, flags);
+			dsp_cmx_transmit(dsp, skb);
+			spin_unlock_irqrestore(&dsp_lock, flags);
+		}
+		break;
+	case (PH_CONTROL_REQ):
+		spin_lock_irqsave(&dsp_lock, flags);
+		ret = dsp_control_req(dsp, hh, skb);
+		spin_unlock_irqrestore(&dsp_lock, flags);
+		break;
+	case (DL_ESTABLISH_REQ):
+	case (PH_ACTIVATE_REQ):
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: activating b_channel %s\n",
+				__func__, dsp->name);
+		if (dsp->dtmf.hardware || dsp->dtmf.software)
+			dsp_dtmf_goertzel_init(dsp);
+		get_features(ch);
+		/* send ph_activate */
+		hh->prim = PH_ACTIVATE_REQ;
+		if (ch->peer)
+			return ch->recv(ch->peer, skb);
+		break;
+	case (DL_RELEASE_REQ):
+	case (PH_DEACTIVATE_REQ):
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: releasing b_channel %s\n",
+				__func__, dsp->name);
+		spin_lock_irqsave(&dsp_lock, flags);
+		dsp->tone.tone = 0;
+		dsp->tone.hardware = 0;
+		dsp->tone.software = 0;
+		if (timer_pending(&dsp->tone.tl))
+			del_timer(&dsp->tone.tl);
+		if (dsp->conf)
+			dsp_cmx_conf(dsp, 0); /* dsp_cmx_hardware will also be
+						 called here */
+		skb_queue_purge(&dsp->sendq);
+		spin_unlock_irqrestore(&dsp_lock, flags);
+		hh->prim = PH_DEACTIVATE_REQ;
+		if (ch->peer)
+			return ch->recv(ch->peer, skb);
+		break;
+	default:
+		if (dsp_debug & DEBUG_DSP_CORE)
+			printk(KERN_DEBUG "%s: msg %x unhandled %s\n",
+				__func__, hh->prim, dsp->name);
+		ret = -EINVAL;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int
+dsp_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct dsp		*dsp = container_of(ch, struct dsp, ch);
+	u_long		flags;
+	int		err = 0;
+
+	if (debug & DEBUG_DSP_CTRL)
+	printk(KERN_DEBUG "%s:(%x)\n", __func__, cmd);
+
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		break;
+	case CLOSE_CHANNEL:
+		if (dsp->ch.peer)
+			dsp->ch.peer->ctrl(dsp->ch.peer, CLOSE_CHANNEL, NULL);
+
+		/* wait until workqueue has finished,
+		 * must lock here, or we may hit send-process currently
+		 * queueing. */
+		spin_lock_irqsave(&dsp_lock, flags);
+		dsp->b_active = 0;
+		spin_unlock_irqrestore(&dsp_lock, flags);
+		/* MUST not be locked, because it waits until queue is done. */
+		cancel_work_sync(&dsp->workq);
+		spin_lock_irqsave(&dsp_lock, flags);
+		if (timer_pending(&dsp->tone.tl))
+			del_timer(&dsp->tone.tl);
+		skb_queue_purge(&dsp->sendq);
+		if (dsp_debug & DEBUG_DSP_CTRL)
+			printk(KERN_DEBUG "%s: releasing member %s\n",
+				__func__, dsp->name);
+		dsp->b_active = 0;
+		dsp_cmx_conf(dsp, 0); /* dsp_cmx_hardware will also be called
+					 here */
+		dsp_pipeline_destroy(&dsp->pipeline);
+
+		if (dsp_debug & DEBUG_DSP_CTRL)
+			printk(KERN_DEBUG "%s: remove & destroy object %s\n",
+				__func__, dsp->name);
+		list_del(&dsp->list);
+		spin_unlock_irqrestore(&dsp_lock, flags);
+
+		if (dsp_debug & DEBUG_DSP_CTRL)
+			printk(KERN_DEBUG "%s: dsp instance released\n",
+				__func__);
+		vfree(dsp);
+		module_put(THIS_MODULE);
+		break;
+	}
+	return err;
+}
+
+static void
+dsp_send_bh(struct work_struct *work)
+{
+	struct dsp *dsp = container_of(work, struct dsp, workq);
+	struct sk_buff *skb;
+	struct mISDNhead	*hh;
+
+	if (dsp->hdlc && dsp->data_pending)
+		return; /* wait until data has been acknowledged */
+
+	/* send queued data */
+	while ((skb = skb_dequeue(&dsp->sendq))) {
+		/* in locked date, we must have still data in queue */
+		if (dsp->data_pending) {
+			if (dsp_debug & DEBUG_DSP_CORE)
+				printk(KERN_DEBUG "%s: fifo full %s, this is "
+					"no bug!\n", __func__, dsp->name);
+			/* flush transparent data, if not acked */
+			dev_kfree_skb(skb);
+			continue;
+		}
+		hh = mISDN_HEAD_P(skb);
+		if (hh->prim == DL_DATA_REQ) {
+			/* send packet up */
+			if (dsp->up) {
+				if (dsp->up->send(dsp->up, skb))
+					dev_kfree_skb(skb);
+			} else
+				dev_kfree_skb(skb);
+		} else {
+			/* send packet down */
+			if (dsp->ch.peer) {
+				dsp->data_pending = 1;
+				if (dsp->ch.recv(dsp->ch.peer, skb)) {
+					dev_kfree_skb(skb);
+					dsp->data_pending = 0;
+				}
+			} else
+				dev_kfree_skb(skb);
+		}
+	}
+}
+
+static int
+dspcreate(struct channel_req *crq)
+{
+	struct dsp		*ndsp;
+	u_long		flags;
+
+	if (crq->protocol != ISDN_P_B_L2DSP
+	 && crq->protocol != ISDN_P_B_L2DSPHDLC)
+		return -EPROTONOSUPPORT;
+	ndsp = vmalloc(sizeof(struct dsp));
+	if (!ndsp) {
+		printk(KERN_ERR "%s: vmalloc struct dsp failed\n", __func__);
+		return -ENOMEM;
+	}
+	memset(ndsp, 0, sizeof(struct dsp));
+	if (dsp_debug & DEBUG_DSP_CTRL)
+		printk(KERN_DEBUG "%s: creating new dsp instance\n", __func__);
+
+	/* default enabled */
+	INIT_WORK(&ndsp->workq, (void *)dsp_send_bh);
+	skb_queue_head_init(&ndsp->sendq);
+	ndsp->ch.send = dsp_function;
+	ndsp->ch.ctrl = dsp_ctrl;
+	ndsp->up = crq->ch;
+	crq->ch = &ndsp->ch;
+	if (crq->protocol == ISDN_P_B_L2DSP) {
+		crq->protocol = ISDN_P_B_RAW;
+		ndsp->hdlc = 0;
+	} else {
+		crq->protocol = ISDN_P_B_HDLC;
+		ndsp->hdlc = 1;
+	}
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n",
+			__func__);
+
+	sprintf(ndsp->name, "DSP_C%x(0x%p)",
+		ndsp->up->st->dev->id + 1, ndsp);
+	/* set frame size to start */
+	ndsp->features.hfc_id = -1; /* current PCM id */
+	ndsp->features.pcm_id = -1; /* current PCM id */
+	ndsp->pcm_slot_rx = -1; /* current CPM slot */
+	ndsp->pcm_slot_tx = -1;
+	ndsp->pcm_bank_rx = -1;
+	ndsp->pcm_bank_tx = -1;
+	ndsp->hfc_conf = -1; /* current conference number */
+	/* set tone timer */
+	ndsp->tone.tl.function = (void *)dsp_tone_timeout;
+	ndsp->tone.tl.data = (long) ndsp;
+	init_timer(&ndsp->tone.tl);
+
+	if (dtmfthreshold < 20 || dtmfthreshold > 500)
+		dtmfthreshold = 200;
+	ndsp->dtmf.treshold = dtmfthreshold*10000;
+
+	/* init pipeline append to list */
+	spin_lock_irqsave(&dsp_lock, flags);
+	dsp_pipeline_init(&ndsp->pipeline);
+	list_add_tail(&ndsp->list, &dsp_ilist);
+	spin_unlock_irqrestore(&dsp_lock, flags);
+
+	return 0;
+}
+
+
+static struct Bprotocol DSP = {
+	.Bprotocols = (1 << (ISDN_P_B_L2DSP & ISDN_P_B_MASK))
+		| (1 << (ISDN_P_B_L2DSPHDLC & ISDN_P_B_MASK)),
+	.name = "dsp",
+	.create = dspcreate
+};
+
+static int dsp_init(void)
+{
+	int err;
+	int tics;
+
+	printk(KERN_INFO "DSP modul %s\n", mISDN_dsp_revision);
+
+	dsp_options = options;
+	dsp_debug = debug;
+
+	/* set packet size */
+	dsp_poll = poll;
+	if (dsp_poll) {
+		if (dsp_poll > MAX_POLL) {
+			printk(KERN_ERR "%s: Wrong poll value (%d), use %d "
+				"maximum.\n", __func__, poll, MAX_POLL);
+			err = -EINVAL;
+			return err;
+		}
+		if (dsp_poll < 8) {
+			printk(KERN_ERR "%s: Wrong poll value (%d), use 8 "
+				"minimum.\n", __func__, dsp_poll);
+			err = -EINVAL;
+			return err;
+		}
+		dsp_tics = poll * HZ / 8000;
+		if (dsp_tics * 8000 != poll * HZ) {
+			printk(KERN_INFO "mISDN_dsp: Cannot clock every %d "
+				"samples (0,125 ms). It is not a multiple of "
+				"%d HZ.\n", poll, HZ);
+			err = -EINVAL;
+			return err;
+		}
+	} else {
+		poll = 8;
+		while (poll <= MAX_POLL) {
+			tics = poll * HZ / 8000;
+			if (tics * 8000 == poll * HZ) {
+				dsp_tics = tics;
+				dsp_poll = poll;
+				if (poll >= 64)
+					break;
+			}
+			poll++;
+		}
+	}
+	if (dsp_poll == 0) {
+		printk(KERN_INFO "mISDN_dsp: There is no multiple of kernel "
+			"clock that equals exactly the duration of 8-256 "
+			"samples. (Choose kernel clock speed like 100, 250, "
+			"300, 1000)\n");
+		err = -EINVAL;
+		return err;
+	}
+	printk(KERN_INFO "mISDN_dsp: DSP clocks every %d samples. This equals "
+		"%d jiffies.\n", dsp_poll, dsp_tics);
+
+	spin_lock_init(&dsp_lock);
+	INIT_LIST_HEAD(&dsp_ilist);
+	INIT_LIST_HEAD(&conf_ilist);
+
+	/* init conversion tables */
+	dsp_audio_generate_law_tables();
+	dsp_silence = (dsp_options&DSP_OPT_ULAW)?0xff:0x2a;
+	dsp_audio_law_to_s32 = (dsp_options&DSP_OPT_ULAW)?dsp_audio_ulaw_to_s32:
+		dsp_audio_alaw_to_s32;
+	dsp_audio_generate_s2law_table();
+	dsp_audio_generate_seven();
+	dsp_audio_generate_mix_table();
+	if (dsp_options & DSP_OPT_ULAW)
+		dsp_audio_generate_ulaw_samples();
+	dsp_audio_generate_volume_changes();
+
+	err = dsp_pipeline_module_init();
+	if (err) {
+		printk(KERN_ERR "mISDN_dsp: Can't initialize pipeline, "
+			"error(%d)\n", err);
+		return err;
+	}
+
+	err = mISDN_register_Bprotocol(&DSP);
+	if (err) {
+		printk(KERN_ERR "Can't register %s error(%d)\n", DSP.name, err);
+		return err;
+	}
+
+	/* set sample timer */
+	dsp_spl_tl.function = (void *)dsp_cmx_send;
+	dsp_spl_tl.data = 0;
+	init_timer(&dsp_spl_tl);
+	dsp_spl_tl.expires = jiffies + dsp_tics;
+	dsp_spl_jiffies = dsp_spl_tl.expires;
+	add_timer(&dsp_spl_tl);
+
+	return 0;
+}
+
+
+static void dsp_cleanup(void)
+{
+	mISDN_unregister_Bprotocol(&DSP);
+
+	if (timer_pending(&dsp_spl_tl))
+		del_timer(&dsp_spl_tl);
+
+	if (!list_empty(&dsp_ilist)) {
+		printk(KERN_ERR "mISDN_dsp: Audio DSP object inst list not "
+			"empty.\n");
+	}
+	if (!list_empty(&conf_ilist)) {
+		printk(KERN_ERR "mISDN_dsp: Conference list not empty. Not "
+			"all memory freed.\n");
+	}
+
+	dsp_pipeline_module_exit();
+}
+
+module_init(dsp_init);
+module_exit(dsp_cleanup);
+
diff --git a/drivers/isdn/mISDN/dsp_dtmf.c b/drivers/isdn/mISDN/dsp_dtmf.c
new file mode 100644
index 00000000000..efc371c1f0d
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_dtmf.c
@@ -0,0 +1,303 @@
+/*
+ * DTMF decoder.
+ *
+ * Copyright            by Andreas Eversberg (jolly@eversberg.eu)
+ *			based on different decoders such as ISDN4Linux
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "core.h"
+#include "dsp.h"
+
+#define NCOEFF            8     /* number of frequencies to be analyzed */
+
+/* For DTMF recognition:
+ * 2 * cos(2 * PI * k / N) precalculated for all k
+ */
+static u64 cos2pik[NCOEFF] =
+{
+	/* k << 15 (source: hfc-4s/8s documentation (www.colognechip.de)) */
+	55960, 53912, 51402, 48438, 38146, 32650, 26170, 18630
+};
+
+/* digit matrix */
+static char dtmf_matrix[4][4] =
+{
+	{'1', '2', '3', 'A'},
+	{'4', '5', '6', 'B'},
+	{'7', '8', '9', 'C'},
+	{'*', '0', '#', 'D'}
+};
+
+/* dtmf detection using goertzel algorithm
+ * init function
+ */
+void dsp_dtmf_goertzel_init(struct dsp *dsp)
+{
+	dsp->dtmf.size = 0;
+	dsp->dtmf.lastwhat = '\0';
+	dsp->dtmf.lastdigit = '\0';
+	dsp->dtmf.count = 0;
+}
+
+/* check for hardware or software features
+ */
+void dsp_dtmf_hardware(struct dsp *dsp)
+{
+	int hardware = 1;
+
+	if (!dsp->features.hfc_dtmf)
+		hardware = 0;
+
+	/* check for volume change */
+	if (dsp->tx_volume) {
+		if (dsp_debug & DEBUG_DSP_DTMF)
+			printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, "
+				"because tx_volume is changed\n",
+				__func__, dsp->name);
+		hardware = 0;
+	}
+	if (dsp->rx_volume) {
+		if (dsp_debug & DEBUG_DSP_DTMF)
+			printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, "
+				"because rx_volume is changed\n",
+				__func__, dsp->name);
+		hardware = 0;
+	}
+	/* check if encryption is enabled */
+	if (dsp->bf_enable) {
+		if (dsp_debug & DEBUG_DSP_DTMF)
+			printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, "
+				"because encryption is enabled\n",
+				__func__, dsp->name);
+		hardware = 0;
+	}
+	/* check if pipeline exists */
+	if (dsp->pipeline.inuse) {
+		if (dsp_debug & DEBUG_DSP_DTMF)
+			printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, "
+				"because pipeline exists.\n",
+				__func__, dsp->name);
+		hardware = 0;
+	}
+
+	dsp->dtmf.hardware = hardware;
+	dsp->dtmf.software = !hardware;
+}
+
+
+/*************************************************************
+ * calculate the coefficients of the given sample and decode *
+ *************************************************************/
+
+/* the given sample is decoded. if the sample is not long enough for a
+ * complete frame, the decoding is finished and continued with the next
+ * call of this function.
+ *
+ * the algorithm is very good for detection with a minimum of errors. i
+ * tested it allot. it even works with very short tones (40ms). the only
+ * disadvantage is, that it doesn't work good with different volumes of both
+ * tones. this will happen, if accoustically coupled dialers are used.
+ * it sometimes detects tones during speach, which is normal for decoders.
+ * use sequences to given commands during calls.
+ *
+ * dtmf - points to a structure of the current dtmf state
+ * spl and len - the sample
+ * fmt - 0 = alaw, 1 = ulaw, 2 = coefficients from HFC DTMF hw-decoder
+ */
+
+u8
+*dsp_dtmf_goertzel_decode(struct dsp *dsp, u8 *data, int len, int fmt)
+{
+	u8 what;
+	int size;
+	signed short *buf;
+	s32 sk, sk1, sk2;
+	int k, n, i;
+	s32 *hfccoeff;
+	s32 result[NCOEFF], tresh, treshl;
+	int lowgroup, highgroup;
+	s64 cos2pik_;
+
+	dsp->dtmf.digits[0] = '\0';
+
+	/* Note: The function will loop until the buffer has not enough samples
+	 * left to decode a full frame.
+	 */
+again:
+	/* convert samples */
+	size = dsp->dtmf.size;
+	buf = dsp->dtmf.buffer;
+	switch (fmt) {
+	case 0: /* alaw */
+	case 1: /* ulaw */
+		while (size < DSP_DTMF_NPOINTS && len) {
+			buf[size++] = dsp_audio_law_to_s32[*data++];
+			len--;
+		}
+		break;
+
+	case 2: /* HFC coefficients */
+	default:
+		if (len < 64) {
+			if (len > 0)
+				printk(KERN_ERR "%s: coefficients have invalid "
+					"size. (is=%d < must=%d)\n",
+					__func__, len, 64);
+			return dsp->dtmf.digits;
+		}
+		hfccoeff = (s32 *)data;
+		for (k = 0; k < NCOEFF; k++) {
+			sk2 = (*hfccoeff++)>>4;
+			sk = (*hfccoeff++)>>4;
+			if (sk > 32767 || sk < -32767 || sk2 > 32767
+			    || sk2 < -32767)
+				printk(KERN_WARNING
+					"DTMF-Detection overflow\n");
+			/* compute |X(k)|**2 */
+			result[k] =
+				 (sk * sk) -
+				 (((cos2pik[k] * sk) >> 15) * sk2) +
+				 (sk2 * sk2);
+		}
+		data += 64;
+		len -= 64;
+		goto coefficients;
+		break;
+	}
+	dsp->dtmf.size = size;
+
+	if (size < DSP_DTMF_NPOINTS)
+		return dsp->dtmf.digits;
+
+	dsp->dtmf.size = 0;
+
+	/* now we have a full buffer of signed long samples - we do goertzel */
+	for (k = 0; k < NCOEFF; k++) {
+		sk = 0;
+		sk1 = 0;
+		sk2 = 0;
+		buf = dsp->dtmf.buffer;
+		cos2pik_ = cos2pik[k];
+		for (n = 0; n < DSP_DTMF_NPOINTS; n++) {
+			sk = ((cos2pik_*sk1)>>15) - sk2 + (*buf++);
+			sk2 = sk1;
+			sk1 = sk;
+		}
+		sk >>= 8;
+		sk2 >>= 8;
+		if (sk > 32767 || sk < -32767 || sk2 > 32767 || sk2 < -32767)
+			printk(KERN_WARNING "DTMF-Detection overflow\n");
+		/* compute |X(k)|**2 */
+		result[k] =
+			(sk * sk) -
+			(((cos2pik[k] * sk) >> 15) * sk2) +
+			(sk2 * sk2);
+	}
+
+	/* our (squared) coefficients have been calculated, we need to process
+	 * them.
+	 */
+coefficients:
+	tresh = 0;
+	for (i = 0; i < NCOEFF; i++) {
+		if (result[i] < 0)
+			result[i] = 0;
+		if (result[i] > dsp->dtmf.treshold) {
+			if (result[i] > tresh)
+				tresh = result[i];
+		}
+	}
+
+	if (tresh == 0) {
+		what = 0;
+		goto storedigit;
+	}
+
+	if (dsp_debug & DEBUG_DSP_DTMFCOEFF)
+		printk(KERN_DEBUG "a %3d %3d %3d %3d %3d %3d %3d %3d"
+			" tr:%3d r %3d %3d %3d %3d %3d %3d %3d %3d\n",
+			result[0]/10000, result[1]/10000, result[2]/10000,
+			result[3]/10000, result[4]/10000, result[5]/10000,
+			result[6]/10000, result[7]/10000, tresh/10000,
+			result[0]/(tresh/100), result[1]/(tresh/100),
+			result[2]/(tresh/100), result[3]/(tresh/100),
+			result[4]/(tresh/100), result[5]/(tresh/100),
+			result[6]/(tresh/100), result[7]/(tresh/100));
+
+	/* calc digit (lowgroup/highgroup) */
+	lowgroup = -1;
+	highgroup = -1;
+	treshl = tresh >> 3;  /* tones which are not on, must be below 9 dB */
+	tresh = tresh >> 2;  /* touchtones must match within 6 dB */
+	for (i = 0; i < NCOEFF; i++) {
+		if (result[i] < treshl)
+			continue;  /* ignore */
+		if (result[i] < tresh) {
+			lowgroup = -1;
+			highgroup = -1;
+			break;  /* noise inbetween */
+		}
+		/* good level found. This is allowed only one time per group */
+		if (i < NCOEFF/2) {
+			/* lowgroup */
+			if (lowgroup >= 0) {
+				/* Bad. Another tone found. */
+				lowgroup = -1;
+				break;
+			} else
+				lowgroup = i;
+		} else {
+			/* higroup */
+			if (highgroup >= 0) {
+				/* Bad. Another tone found. */
+				highgroup = -1;
+				break;
+			} else
+				highgroup = i-(NCOEFF/2);
+		}
+	}
+
+	/* get digit or null */
+	what = 0;
+	if (lowgroup >= 0 && highgroup >= 0)
+		what = dtmf_matrix[lowgroup][highgroup];
+
+storedigit:
+	if (what && (dsp_debug & DEBUG_DSP_DTMF))
+		printk(KERN_DEBUG "DTMF what: %c\n", what);
+
+	if (dsp->dtmf.lastwhat != what)
+		dsp->dtmf.count = 0;
+
+	/* the tone (or no tone) must remain 3 times without change */
+	if (dsp->dtmf.count == 2) {
+		if (dsp->dtmf.lastdigit != what) {
+			dsp->dtmf.lastdigit = what;
+			if (what) {
+				if (dsp_debug & DEBUG_DSP_DTMF)
+					printk(KERN_DEBUG "DTMF digit: %c\n",
+						what);
+				if ((strlen(dsp->dtmf.digits)+1)
+					< sizeof(dsp->dtmf.digits)) {
+					dsp->dtmf.digits[strlen(
+						dsp->dtmf.digits)+1] = '\0';
+					dsp->dtmf.digits[strlen(
+						dsp->dtmf.digits)] = what;
+				}
+			}
+		}
+	} else
+		dsp->dtmf.count++;
+
+	dsp->dtmf.lastwhat = what;
+
+	goto again;
+}
+
+
diff --git a/drivers/isdn/mISDN/dsp_ecdis.h b/drivers/isdn/mISDN/dsp_ecdis.h
new file mode 100644
index 00000000000..8a20af43308
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_ecdis.h
@@ -0,0 +1,110 @@
+/*
+ * SpanDSP - a series of DSP components for telephony
+ *
+ * ec_disable_detector.h - A detector which should eventually meet the
+ *                         G.164/G.165 requirements for detecting the
+ *                         2100Hz echo cancellor disable tone.
+ *
+ * Written by Steve Underwood <steveu@coppice.org>
+ *
+ * Copyright (C) 2001 Steve Underwood
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include "dsp_biquad.h"
+
+struct ec_disable_detector_state {
+	struct biquad2_state notch;
+	int notch_level;
+	int channel_level;
+	int tone_present;
+	int tone_cycle_duration;
+	int good_cycles;
+	int hit;
+};
+
+
+#define FALSE 0
+#define TRUE (!FALSE)
+
+static inline void
+echo_can_disable_detector_init(struct ec_disable_detector_state *det)
+{
+    /* Elliptic notch */
+    /* This is actually centred at 2095Hz, but gets the balance we want, due
+       to the asymmetric walls of the notch */
+	biquad2_init(&det->notch,
+		(int32_t) (-0.7600000*32768.0),
+		(int32_t) (-0.1183852*32768.0),
+		(int32_t) (-0.5104039*32768.0),
+		(int32_t) (0.1567596*32768.0),
+		(int32_t) (1.0000000*32768.0));
+
+	det->channel_level = 0;
+	det->notch_level = 0;
+	det->tone_present = FALSE;
+	det->tone_cycle_duration = 0;
+	det->good_cycles = 0;
+	det->hit = 0;
+}
+/*- End of function --------------------------------------------------------*/
+
+static inline int
+echo_can_disable_detector_update(struct ec_disable_detector_state *det,
+int16_t amp)
+{
+	int16_t notched;
+
+	notched = biquad2(&det->notch, amp);
+	/* Estimate the overall energy in the channel, and the energy in
+	   the notch (i.e. overall channel energy - tone energy => noise).
+	   Use abs instead of multiply for speed (is it really faster?).
+	   Damp the overall energy a little more for a stable result.
+	   Damp the notch energy a little less, so we don't damp out the
+	   blip every time the phase reverses */
+	det->channel_level += ((abs(amp) - det->channel_level) >> 5);
+	det->notch_level += ((abs(notched) - det->notch_level) >> 4);
+	if (det->channel_level > 280) {
+		/* There is adequate energy in the channel.
+		 Is it mostly at 2100Hz? */
+		if (det->notch_level*6 < det->channel_level) {
+			/* The notch says yes, so we have the tone. */
+			if (!det->tone_present) {
+				/* Do we get a kick every 450+-25ms? */
+				if (det->tone_cycle_duration >= 425*8
+					&& det->tone_cycle_duration <= 475*8) {
+					det->good_cycles++;
+					if (det->good_cycles > 2)
+					det->hit = TRUE;
+				}
+				det->tone_cycle_duration = 0;
+			}
+			det->tone_present = TRUE;
+		} else
+			det->tone_present = FALSE;
+		det->tone_cycle_duration++;
+	} else {
+		det->tone_present = FALSE;
+		det->tone_cycle_duration = 0;
+		det->good_cycles = 0;
+	}
+	return det->hit;
+}
+/*- End of function --------------------------------------------------------*/
+/*- End of file ------------------------------------------------------------*/
diff --git a/drivers/isdn/mISDN/dsp_hwec.c b/drivers/isdn/mISDN/dsp_hwec.c
new file mode 100644
index 00000000000..eb892d9dd5c
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_hwec.c
@@ -0,0 +1,138 @@
+/*
+ * dsp_hwec.c:
+ * builtin mISDN dsp pipeline element for enabling the hw echocanceller
+ *
+ * Copyright (C) 2007, Nadi Sarrar
+ *
+ * Nadi Sarrar <nadi@beronet.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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/mISDNdsp.h>
+#include <linux/mISDNif.h>
+#include "core.h"
+#include "dsp.h"
+#include "dsp_hwec.h"
+
+static struct mISDN_dsp_element_arg args[] = {
+	{ "deftaps", "128", "Set the number of taps of cancellation." },
+};
+
+static struct mISDN_dsp_element dsp_hwec_p = {
+	.name = "hwec",
+	.new = NULL,
+	.free = NULL,
+	.process_tx = NULL,
+	.process_rx = NULL,
+	.num_args = sizeof(args) / sizeof(struct mISDN_dsp_element_arg),
+	.args = args,
+};
+struct mISDN_dsp_element *dsp_hwec = &dsp_hwec_p;
+
+void dsp_hwec_enable(struct dsp *dsp, const char *arg)
+{
+	int deftaps = 128,
+		len;
+	struct mISDN_ctrl_req	cq;
+
+	if (!dsp) {
+		printk(KERN_ERR "%s: failed to enable hwec: dsp is NULL\n",
+			__func__);
+		return;
+	}
+
+	if (!arg)
+		goto _do;
+
+	len = strlen(arg);
+	if (!len)
+		goto _do;
+
+	{
+		char _dup[len + 1];
+		char *dup, *tok, *name, *val;
+		int tmp;
+
+		strcpy(_dup, arg);
+		dup = _dup;
+
+		while ((tok = strsep(&dup, ","))) {
+			if (!strlen(tok))
+				continue;
+			name = strsep(&tok, "=");
+			val = tok;
+
+			if (!val)
+				continue;
+
+			if (!strcmp(name, "deftaps")) {
+				if (sscanf(val, "%d", &tmp) == 1)
+					deftaps = tmp;
+			}
+		}
+	}
+
+_do:
+	printk(KERN_DEBUG "%s: enabling hwec with deftaps=%d\n",
+		__func__, deftaps);
+	memset(&cq, 0, sizeof(cq));
+	cq.op = MISDN_CTRL_HFC_ECHOCAN_ON;
+	cq.p1 = deftaps;
+	if (!dsp->ch.peer->ctrl(&dsp->ch, CONTROL_CHANNEL, &cq)) {
+		printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n",
+			__func__);
+		return;
+	}
+}
+
+void dsp_hwec_disable(struct dsp *dsp)
+{
+	struct mISDN_ctrl_req	cq;
+
+	if (!dsp) {
+		printk(KERN_ERR "%s: failed to disable hwec: dsp is NULL\n",
+			__func__);
+		return;
+	}
+
+	printk(KERN_DEBUG "%s: disabling hwec\n", __func__);
+	memset(&cq, 0, sizeof(cq));
+	cq.op = MISDN_CTRL_HFC_ECHOCAN_OFF;
+	if (!dsp->ch.peer->ctrl(&dsp->ch, CONTROL_CHANNEL, &cq)) {
+		printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n",
+			__func__);
+		return;
+	}
+}
+
+int dsp_hwec_init(void)
+{
+	mISDN_dsp_element_register(dsp_hwec);
+
+	return 0;
+}
+
+void dsp_hwec_exit(void)
+{
+	mISDN_dsp_element_unregister(dsp_hwec);
+}
+
diff --git a/drivers/isdn/mISDN/dsp_hwec.h b/drivers/isdn/mISDN/dsp_hwec.h
new file mode 100644
index 00000000000..eebe80c3f71
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_hwec.h
@@ -0,0 +1,10 @@
+/*
+ * dsp_hwec.h
+ */
+
+extern struct mISDN_dsp_element *dsp_hwec;
+extern void dsp_hwec_enable(struct dsp *dsp, const char *arg);
+extern void dsp_hwec_disable(struct dsp *dsp);
+extern int  dsp_hwec_init(void);
+extern void dsp_hwec_exit(void);
+
diff --git a/drivers/isdn/mISDN/dsp_pipeline.c b/drivers/isdn/mISDN/dsp_pipeline.c
new file mode 100644
index 00000000000..850260ab57d
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_pipeline.c
@@ -0,0 +1,348 @@
+/*
+ * dsp_pipeline.c: pipelined audio processing
+ *
+ * Copyright (C) 2007, Nadi Sarrar
+ *
+ * Nadi Sarrar <nadi@beronet.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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/string.h>
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "dsp.h"
+#include "dsp_hwec.h"
+
+/* uncomment for debugging */
+/*#define PIPELINE_DEBUG*/
+
+struct dsp_pipeline_entry {
+	struct mISDN_dsp_element *elem;
+	void                *p;
+	struct list_head     list;
+};
+struct dsp_element_entry {
+	struct mISDN_dsp_element *elem;
+	struct device	     dev;
+	struct list_head     list;
+};
+
+static LIST_HEAD(dsp_elements);
+
+/* sysfs */
+static struct class *elements_class;
+
+static ssize_t
+attr_show_args(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct mISDN_dsp_element *elem = dev_get_drvdata(dev);
+	ssize_t len = 0;
+	int i = 0;
+
+	*buf = 0;
+	for (; i < elem->num_args; ++i)
+		len = sprintf(buf, "%sName:        %s\n%s%s%sDescription: %s\n"
+			"\n", buf,
+			  elem->args[i].name,
+			  elem->args[i].def ? "Default:     " : "",
+			  elem->args[i].def ? elem->args[i].def : "",
+			  elem->args[i].def ? "\n" : "",
+			  elem->args[i].desc);
+
+	return len;
+}
+
+static struct device_attribute element_attributes[] = {
+	__ATTR(args, 0444, attr_show_args, NULL),
+};
+
+int mISDN_dsp_element_register(struct mISDN_dsp_element *elem)
+{
+	struct dsp_element_entry *entry;
+	int ret, i;
+
+	if (!elem)
+		return -EINVAL;
+
+	entry = kzalloc(sizeof(struct dsp_element_entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	entry->elem = elem;
+
+	entry->dev.class = elements_class;
+	dev_set_drvdata(&entry->dev, elem);
+	snprintf(entry->dev.bus_id, BUS_ID_SIZE, elem->name);
+	ret = device_register(&entry->dev);
+	if (ret) {
+		printk(KERN_ERR "%s: failed to register %s\n",
+			__func__, elem->name);
+		goto err1;
+	}
+
+	for (i = 0; i < (sizeof(element_attributes)
+		/ sizeof(struct device_attribute)); ++i)
+		ret = device_create_file(&entry->dev,
+				&element_attributes[i]);
+		if (ret) {
+			printk(KERN_ERR "%s: failed to create device file\n",
+				__func__);
+			goto err2;
+		}
+
+	list_add_tail(&entry->list, &dsp_elements);
+
+	printk(KERN_DEBUG "%s: %s registered\n", __func__, elem->name);
+
+	return 0;
+
+err2:
+	device_unregister(&entry->dev);
+err1:
+	kfree(entry);
+	return ret;
+}
+EXPORT_SYMBOL(mISDN_dsp_element_register);
+
+void mISDN_dsp_element_unregister(struct mISDN_dsp_element *elem)
+{
+	struct dsp_element_entry *entry, *n;
+
+	if (!elem)
+		return;
+
+	list_for_each_entry_safe(entry, n, &dsp_elements, list)
+		if (entry->elem == elem) {
+			list_del(&entry->list);
+			device_unregister(&entry->dev);
+			kfree(entry);
+			printk(KERN_DEBUG "%s: %s unregistered\n",
+				__func__, elem->name);
+			return;
+		}
+	printk(KERN_ERR "%s: element %s not in list.\n", __func__, elem->name);
+}
+EXPORT_SYMBOL(mISDN_dsp_element_unregister);
+
+int dsp_pipeline_module_init(void)
+{
+	elements_class = class_create(THIS_MODULE, "dsp_pipeline");
+	if (IS_ERR(elements_class))
+		return PTR_ERR(elements_class);
+
+#ifdef PIPELINE_DEBUG
+	printk(KERN_DEBUG "%s: dsp pipeline module initialized\n", __func__);
+#endif
+
+	dsp_hwec_init();
+
+	return 0;
+}
+
+void dsp_pipeline_module_exit(void)
+{
+	struct dsp_element_entry *entry, *n;
+
+	dsp_hwec_exit();
+
+	class_destroy(elements_class);
+
+	list_for_each_entry_safe(entry, n, &dsp_elements, list) {
+		list_del(&entry->list);
+		printk(KERN_WARNING "%s: element was still registered: %s\n",
+			__func__, entry->elem->name);
+		kfree(entry);
+	}
+
+	printk(KERN_DEBUG "%s: dsp pipeline module exited\n", __func__);
+}
+
+int dsp_pipeline_init(struct dsp_pipeline *pipeline)
+{
+	if (!pipeline)
+		return -EINVAL;
+
+	INIT_LIST_HEAD(&pipeline->list);
+
+#ifdef PIPELINE_DEBUG
+	printk(KERN_DEBUG "%s: dsp pipeline ready\n", __func__);
+#endif
+
+	return 0;
+}
+
+static inline void _dsp_pipeline_destroy(struct dsp_pipeline *pipeline)
+{
+	struct dsp_pipeline_entry *entry, *n;
+
+	list_for_each_entry_safe(entry, n, &pipeline->list, list) {
+		list_del(&entry->list);
+		if (entry->elem == dsp_hwec)
+			dsp_hwec_disable(container_of(pipeline, struct dsp,
+				pipeline));
+		else
+			entry->elem->free(entry->p);
+		kfree(entry);
+	}
+}
+
+void dsp_pipeline_destroy(struct dsp_pipeline *pipeline)
+{
+
+	if (!pipeline)
+		return;
+
+	_dsp_pipeline_destroy(pipeline);
+
+#ifdef PIPELINE_DEBUG
+	printk(KERN_DEBUG "%s: dsp pipeline destroyed\n", __func__);
+#endif
+}
+
+int dsp_pipeline_build(struct dsp_pipeline *pipeline, const char *cfg)
+{
+	int len, incomplete = 0, found = 0;
+	char *dup, *tok, *name, *args;
+	struct dsp_element_entry *entry, *n;
+	struct dsp_pipeline_entry *pipeline_entry;
+	struct mISDN_dsp_element *elem;
+
+	if (!pipeline)
+		return -EINVAL;
+
+	if (!list_empty(&pipeline->list))
+		_dsp_pipeline_destroy(pipeline);
+
+	if (!cfg)
+		return 0;
+
+	len = strlen(cfg);
+	if (!len)
+		return 0;
+
+	dup = kmalloc(len + 1, GFP_KERNEL);
+	if (!dup)
+		return 0;
+	strcpy(dup, cfg);
+	while ((tok = strsep(&dup, "|"))) {
+		if (!strlen(tok))
+			continue;
+		name = strsep(&tok, "(");
+		args = strsep(&tok, ")");
+		if (args && !*args)
+			args = 0;
+
+		list_for_each_entry_safe(entry, n, &dsp_elements, list)
+			if (!strcmp(entry->elem->name, name)) {
+				elem = entry->elem;
+
+				pipeline_entry = kmalloc(sizeof(struct
+					dsp_pipeline_entry), GFP_KERNEL);
+				if (!pipeline_entry) {
+					printk(KERN_DEBUG "%s: failed to add "
+					    "entry to pipeline: %s (out of "
+					    "memory)\n", __func__, elem->name);
+					incomplete = 1;
+					goto _out;
+				}
+				pipeline_entry->elem = elem;
+
+				if (elem == dsp_hwec) {
+					/* This is a hack to make the hwec
+					   available as a pipeline module */
+					dsp_hwec_enable(container_of(pipeline,
+						struct dsp, pipeline), args);
+					list_add_tail(&pipeline_entry->list,
+						&pipeline->list);
+				} else {
+					pipeline_entry->p = elem->new(args);
+					if (pipeline_entry->p) {
+						list_add_tail(&pipeline_entry->
+							list, &pipeline->list);
+#ifdef PIPELINE_DEBUG
+						printk(KERN_DEBUG "%s: created "
+						    "instance of %s%s%s\n",
+						    __func__, name, args ?
+						    " with args " : "", args ?
+						    args : "");
+#endif
+					} else {
+						printk(KERN_DEBUG "%s: failed "
+						  "to add entry to pipeline: "
+						  "%s (new() returned NULL)\n",
+						  __func__, elem->name);
+						kfree(pipeline_entry);
+						incomplete = 1;
+					}
+				}
+				found = 1;
+				break;
+			}
+
+		if (found)
+			found = 0;
+		else {
+			printk(KERN_DEBUG "%s: element not found, skipping: "
+				"%s\n", __func__, name);
+			incomplete = 1;
+		}
+	}
+
+_out:
+	if (!list_empty(&pipeline->list))
+		pipeline->inuse = 1;
+	else
+		pipeline->inuse = 0;
+
+#ifdef PIPELINE_DEBUG
+	printk(KERN_DEBUG "%s: dsp pipeline built%s: %s\n",
+		__func__, incomplete ? " incomplete" : "", cfg);
+#endif
+	kfree(dup);
+	return 0;
+}
+
+void dsp_pipeline_process_tx(struct dsp_pipeline *pipeline, u8 *data, int len)
+{
+	struct dsp_pipeline_entry *entry;
+
+	if (!pipeline)
+		return;
+
+	list_for_each_entry(entry, &pipeline->list, list)
+		if (entry->elem->process_tx)
+			entry->elem->process_tx(entry->p, data, len);
+}
+
+void dsp_pipeline_process_rx(struct dsp_pipeline *pipeline, u8 *data, int len)
+{
+	struct dsp_pipeline_entry *entry;
+
+	if (!pipeline)
+		return;
+
+	list_for_each_entry_reverse(entry, &pipeline->list, list)
+		if (entry->elem->process_rx)
+			entry->elem->process_rx(entry->p, data, len);
+}
+
+
diff --git a/drivers/isdn/mISDN/dsp_tones.c b/drivers/isdn/mISDN/dsp_tones.c
new file mode 100644
index 00000000000..23dd0dd2152
--- /dev/null
+++ b/drivers/isdn/mISDN/dsp_tones.c
@@ -0,0 +1,551 @@
+/*
+ * Audio support data for ISDN4Linux.
+ *
+ * Copyright Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include <linux/mISDNdsp.h>
+#include "core.h"
+#include "dsp.h"
+
+
+#define DATA_S sample_silence
+#define SIZE_S (&sizeof_silence)
+#define DATA_GA sample_german_all
+#define SIZE_GA (&sizeof_german_all)
+#define DATA_GO sample_german_old
+#define SIZE_GO (&sizeof_german_old)
+#define DATA_DT sample_american_dialtone
+#define SIZE_DT (&sizeof_american_dialtone)
+#define DATA_RI sample_american_ringing
+#define SIZE_RI (&sizeof_american_ringing)
+#define DATA_BU sample_american_busy
+#define SIZE_BU (&sizeof_american_busy)
+#define DATA_S1 sample_special1
+#define SIZE_S1 (&sizeof_special1)
+#define DATA_S2 sample_special2
+#define SIZE_S2 (&sizeof_special2)
+#define DATA_S3 sample_special3
+#define SIZE_S3 (&sizeof_special3)
+
+/***************/
+/* tones loops */
+/***************/
+
+/* all tones are alaw encoded */
+/* the last sample+1 is in phase with the first sample. the error is low */
+
+static u8 sample_german_all[] = {
+	0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d,
+	0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c,
+	0xdc, 0xfc, 0x6c,
+	0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d,
+	0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c,
+	0xdc, 0xfc, 0x6c,
+	0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d,
+	0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c,
+	0xdc, 0xfc, 0x6c,
+	0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d,
+	0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c,
+	0xdc, 0xfc, 0x6c,
+};
+static u32 sizeof_german_all = sizeof(sample_german_all);
+
+static u8 sample_german_old[] = {
+	0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed,
+	0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70,
+	0x8c,
+	0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed,
+	0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70,
+	0x8c,
+	0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed,
+	0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70,
+	0x8c,
+	0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed,
+	0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70,
+	0x8c,
+};
+static u32 sizeof_german_old = sizeof(sample_german_old);
+
+static u8 sample_american_dialtone[] = {
+	0x2a, 0x18, 0x90, 0x6c, 0x4c, 0xbc, 0x4c, 0x6c,
+	0x10, 0x58, 0x32, 0xb9, 0x31, 0x2d, 0x8d, 0x0d,
+	0x8d, 0x2d, 0x31, 0x99, 0x0f, 0x28, 0x60, 0xf0,
+	0xd0, 0x50, 0xd0, 0x30, 0x60, 0x08, 0x8e, 0x67,
+	0x09, 0x19, 0x21, 0xe1, 0xd9, 0xb9, 0x29, 0x67,
+	0x83, 0x02, 0xce, 0xbe, 0xee, 0x1a, 0x1b, 0xef,
+	0xbf, 0xcf, 0x03, 0x82, 0x66, 0x28, 0xb8, 0xd8,
+	0xe0, 0x20, 0x18, 0x08, 0x66, 0x8f, 0x09, 0x61,
+	0x31, 0xd1, 0x51, 0xd1, 0xf1, 0x61, 0x29, 0x0e,
+	0x98, 0x30, 0x2c, 0x8c, 0x0c, 0x8c, 0x2c, 0x30,
+	0xb8, 0x33, 0x59, 0x11, 0x6d, 0x4d, 0xbd, 0x4d,
+	0x6d, 0x91, 0x19,
+};
+static u32 sizeof_american_dialtone = sizeof(sample_american_dialtone);
+
+static u8 sample_american_ringing[] = {
+	0x2a, 0xe0, 0xac, 0x0c, 0xbc, 0x4c, 0x8c, 0x90,
+	0x48, 0xc7, 0xc1, 0xed, 0xcd, 0x4d, 0xcd, 0xed,
+	0xc1, 0xb7, 0x08, 0x30, 0xec, 0xcc, 0xcc, 0x8c,
+	0x10, 0x58, 0x1a, 0x99, 0x71, 0xed, 0x8d, 0x8d,
+	0x2d, 0x41, 0x89, 0x9e, 0x20, 0x70, 0x2c, 0xec,
+	0x2c, 0x70, 0x20, 0x86, 0x77, 0xe1, 0x31, 0x11,
+	0xd1, 0xf1, 0x81, 0x09, 0xa3, 0x56, 0x58, 0x00,
+	0x40, 0xc0, 0x60, 0x38, 0x46, 0x43, 0x57, 0x39,
+	0xd9, 0x59, 0x99, 0xc9, 0x77, 0x2f, 0x2e, 0xc6,
+	0xd6, 0x28, 0xd6, 0x36, 0x26, 0x2e, 0x8a, 0xa3,
+	0x43, 0x63, 0x4b, 0x4a, 0x62, 0x42, 0xa2, 0x8b,
+	0x2f, 0x27, 0x37, 0xd7, 0x29, 0xd7, 0xc7, 0x2f,
+	0x2e, 0x76, 0xc8, 0x98, 0x58, 0xd8, 0x38, 0x56,
+	0x42, 0x47, 0x39, 0x61, 0xc1, 0x41, 0x01, 0x59,
+	0x57, 0xa2, 0x08, 0x80, 0xf0, 0xd0, 0x10, 0x30,
+	0xe0, 0x76, 0x87, 0x21, 0x71, 0x2d, 0xed, 0x2d,
+	0x71, 0x21, 0x9f, 0x88, 0x40, 0x2c, 0x8c, 0x8c,
+	0xec, 0x70, 0x98, 0x1b, 0x59, 0x11, 0x8d, 0xcd,
+	0xcd, 0xed, 0x31, 0x09, 0xb6, 0xc0, 0xec, 0xcc,
+	0x4c, 0xcc, 0xec, 0xc0, 0xc6, 0x49, 0x91, 0x8d,
+	0x4d, 0xbd, 0x0d, 0xad, 0xe1,
+};
+static u32 sizeof_american_ringing = sizeof(sample_american_ringing);
+
+static u8 sample_american_busy[] = {
+	0x2a, 0x00, 0x6c, 0x4c, 0x4c, 0x6c, 0xb0, 0x66,
+	0x99, 0x11, 0x6d, 0x8d, 0x2d, 0x41, 0xd7, 0x96,
+	0x60, 0xf0, 0x70, 0x40, 0x58, 0xf6, 0x53, 0x57,
+	0x09, 0x89, 0xd7, 0x5f, 0xe3, 0x2a, 0xe3, 0x5f,
+	0xd7, 0x89, 0x09, 0x57, 0x53, 0xf6, 0x58, 0x40,
+	0x70, 0xf0, 0x60, 0x96, 0xd7, 0x41, 0x2d, 0x8d,
+	0x6d, 0x11, 0x99, 0x66, 0xb0, 0x6c, 0x4c, 0x4c,
+	0x6c, 0x00, 0x2a, 0x01, 0x6d, 0x4d, 0x4d, 0x6d,
+	0xb1, 0x67, 0x98, 0x10, 0x6c, 0x8c, 0x2c, 0x40,
+	0xd6, 0x97, 0x61, 0xf1, 0x71, 0x41, 0x59, 0xf7,
+	0x52, 0x56, 0x08, 0x88, 0xd6, 0x5e, 0xe2, 0x2a,
+	0xe2, 0x5e, 0xd6, 0x88, 0x08, 0x56, 0x52, 0xf7,
+	0x59, 0x41, 0x71, 0xf1, 0x61, 0x97, 0xd6, 0x40,
+	0x2c, 0x8c, 0x6c, 0x10, 0x98, 0x67, 0xb1, 0x6d,
+	0x4d, 0x4d, 0x6d, 0x01,
+};
+static u32 sizeof_american_busy = sizeof(sample_american_busy);
+
+static u8 sample_special1[] = {
+	0x2a, 0x2c, 0xbc, 0x6c, 0xd6, 0x71, 0xbd, 0x0d,
+	0xd9, 0x80, 0xcc, 0x4c, 0x40, 0x39, 0x0d, 0xbd,
+	0x11, 0x86, 0xec, 0xbc, 0xec, 0x0e, 0x51, 0xbd,
+	0x8d, 0x89, 0x30, 0x4c, 0xcc, 0xe0, 0xe1, 0xcd,
+	0x4d, 0x31, 0x88, 0x8c, 0xbc, 0x50, 0x0f, 0xed,
+	0xbd, 0xed, 0x87, 0x10, 0xbc, 0x0c, 0x38, 0x41,
+	0x4d, 0xcd, 0x81, 0xd8, 0x0c, 0xbc, 0x70, 0xd7,
+	0x6d, 0xbd, 0x2d,
+};
+static u32 sizeof_special1 = sizeof(sample_special1);
+
+static u8 sample_special2[] = {
+	0x2a, 0xcc, 0x8c, 0xd7, 0x4d, 0x2d, 0x18, 0xbc,
+	0x10, 0xc1, 0xbd, 0xc1, 0x10, 0xbc, 0x18, 0x2d,
+	0x4d, 0xd7, 0x8c, 0xcc, 0x2a, 0xcd, 0x8d, 0xd6,
+	0x4c, 0x2c, 0x19, 0xbd, 0x11, 0xc0, 0xbc, 0xc0,
+	0x11, 0xbd, 0x19, 0x2c, 0x4c, 0xd6, 0x8d, 0xcd,
+	0x2a, 0xcc, 0x8c, 0xd7, 0x4d, 0x2d, 0x18, 0xbc,
+	0x10, 0xc1, 0xbd, 0xc1, 0x10, 0xbc, 0x18, 0x2d,
+	0x4d, 0xd7, 0x8c, 0xcc, 0x2a, 0xcd, 0x8d, 0xd6,
+	0x4c, 0x2c, 0x19, 0xbd, 0x11, 0xc0, 0xbc, 0xc0,
+	0x11, 0xbd, 0x19, 0x2c, 0x4c, 0xd6, 0x8d, 0xcd,
+};
+static u32 sizeof_special2 = sizeof(sample_special2);
+
+static u8 sample_special3[] = {
+	0x2a, 0xbc, 0x18, 0xcd, 0x11, 0x2c, 0x8c, 0xc1,
+	0x4d, 0xd6, 0xbc, 0xd6, 0x4d, 0xc1, 0x8c, 0x2c,
+	0x11, 0xcd, 0x18, 0xbc, 0x2a, 0xbd, 0x19, 0xcc,
+	0x10, 0x2d, 0x8d, 0xc0, 0x4c, 0xd7, 0xbd, 0xd7,
+	0x4c, 0xc0, 0x8d, 0x2d, 0x10, 0xcc, 0x19, 0xbd,
+	0x2a, 0xbc, 0x18, 0xcd, 0x11, 0x2c, 0x8c, 0xc1,
+	0x4d, 0xd6, 0xbc, 0xd6, 0x4d, 0xc1, 0x8c, 0x2c,
+	0x11, 0xcd, 0x18, 0xbc, 0x2a, 0xbd, 0x19, 0xcc,
+	0x10, 0x2d, 0x8d, 0xc0, 0x4c, 0xd7, 0xbd, 0xd7,
+	0x4c, 0xc0, 0x8d, 0x2d, 0x10, 0xcc, 0x19, 0xbd,
+};
+static u32 sizeof_special3 = sizeof(sample_special3);
+
+static u8 sample_silence[] = {
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+};
+static u32 sizeof_silence = sizeof(sample_silence);
+
+struct tones_samples {
+	u32 *len;
+	u8 *data;
+};
+static struct
+tones_samples samples[] = {
+	{&sizeof_german_all, sample_german_all},
+	{&sizeof_german_old, sample_german_old},
+	{&sizeof_american_dialtone, sample_american_dialtone},
+	{&sizeof_american_ringing, sample_american_ringing},
+	{&sizeof_american_busy, sample_american_busy},
+	{&sizeof_special1, sample_special1},
+	{&sizeof_special2, sample_special2},
+	{&sizeof_special3, sample_special3},
+	{NULL, NULL},
+};
+
+/***********************************
+ * generate ulaw from alaw samples *
+ ***********************************/
+
+void
+dsp_audio_generate_ulaw_samples(void)
+{
+	int i, j;
+
+	i = 0;
+	while (samples[i].len) {
+		j = 0;
+		while (j < (*samples[i].len)) {
+			samples[i].data[j] =
+				dsp_audio_alaw_to_ulaw[samples[i].data[j]];
+			j++;
+		}
+		i++;
+	}
+}
+
+
+/****************************
+ * tone sequence definition *
+ ****************************/
+
+struct pattern {
+	int tone;
+	u8 *data[10];
+	u32 *siz[10];
+	u32 seq[10];
+} pattern[] = {
+	{TONE_GERMAN_DIALTONE,
+	{DATA_GA, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{1900, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDDIALTONE,
+	{DATA_GO, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{1998, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_DIALTONE,
+	{DATA_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{8000, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_DIALPBX,
+	{DATA_GA, DATA_S, DATA_GA, DATA_S, DATA_GA, DATA_S, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, 0, 0, 0, 0},
+	{2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDDIALPBX,
+	{DATA_GO, DATA_S, DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0},
+	{2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_DIALPBX,
+	{DATA_DT, DATA_S, DATA_DT, DATA_S, DATA_DT, DATA_S, 0, 0, 0, 0},
+	{SIZE_DT, SIZE_S, SIZE_DT, SIZE_S, SIZE_DT, SIZE_S, 0, 0, 0, 0},
+	{2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_RINGING,
+	{DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{8000, 32000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDRINGING,
+	{DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{8000, 40000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_RINGING,
+	{DATA_RI, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_RI, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{8000, 32000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_RINGPBX,
+	{DATA_GA, DATA_S, DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDRINGPBX,
+	{DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_RINGPBX,
+	{DATA_RI, DATA_S, DATA_RI, DATA_S, 0, 0, 0, 0, 0, 0},
+	{SIZE_RI, SIZE_S, SIZE_RI, SIZE_S, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_BUSY,
+	{DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDBUSY,
+	{DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{1000, 5000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_BUSY,
+	{DATA_BU, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_BU, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_HANGUP,
+	{DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_OLDHANGUP,
+	{DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{1000, 5000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_AMERICAN_HANGUP,
+	{DATA_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{8000, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_SPECIAL_INFO,
+	{DATA_S1, DATA_S2, DATA_S3, DATA_S, 0, 0, 0, 0, 0, 0},
+	{SIZE_S1, SIZE_S2, SIZE_S3, SIZE_S, 0, 0, 0, 0, 0, 0},
+	{2666, 2666, 2666, 8002, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_GASSENBESETZT,
+	{DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0},
+	{2000, 2000, 0, 0, 0, 0, 0, 0, 0, 0} },
+
+	{TONE_GERMAN_AUFSCHALTTON,
+	{DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0},
+	{SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0},
+	{1000, 5000, 1000, 17000, 0, 0, 0, 0, 0, 0} },
+
+	{0,
+	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+};
+
+/******************
+ * copy tone data *
+ ******************/
+
+/* an sk_buff is generated from the number of samples needed.
+ * the count will be changed and may begin from 0 each pattern period.
+ * the clue is to precalculate the pointers and legths to use only one
+ * memcpy per function call, or two memcpy if the tone sequence changes.
+ *
+ * pattern - the type of the pattern
+ * count - the sample from the beginning of the pattern (phase)
+ * len - the number of bytes
+ *
+ * return - the sk_buff with the sample
+ *
+ * if tones has finished (e.g. knocking tone), dsp->tones is turned off
+ */
+void dsp_tone_copy(struct dsp *dsp, u8 *data, int len)
+{
+	int index, count, start, num;
+	struct pattern *pat;
+	struct dsp_tone *tone = &dsp->tone;
+
+	/* if we have no tone, we copy silence */
+	if (!tone->tone) {
+		memset(data, dsp_silence, len);
+		return;
+	}
+
+	/* process pattern */
+	pat = (struct pattern *)tone->pattern;
+		/* points to the current pattern */
+	index = tone->index; /* gives current sequence index */
+	count = tone->count; /* gives current sample */
+
+	/* copy sample */
+	while (len) {
+		/* find sample to start with */
+		while (42) {
+			/* warp arround */
+			if (!pat->seq[index]) {
+				count = 0;
+				index = 0;
+			}
+			/* check if we are currently playing this tone */
+			if (count < pat->seq[index])
+				break;
+			if (dsp_debug & DEBUG_DSP_TONE)
+				printk(KERN_DEBUG "%s: reaching next sequence "
+					"(index=%d)\n", __func__, index);
+			count -= pat->seq[index];
+			index++;
+		}
+		/* calculate start and number of samples */
+		start = count % (*(pat->siz[index]));
+		num = len;
+		if (num+count > pat->seq[index])
+			num = pat->seq[index] - count;
+		if (num+start > (*(pat->siz[index])))
+			num = (*(pat->siz[index])) - start;
+		/* copy memory */
+		memcpy(data, pat->data[index]+start, num);
+		/* reduce length */
+		data += num;
+		count += num;
+		len -= num;
+	}
+	tone->index = index;
+	tone->count = count;
+
+	/* return sk_buff */
+	return;
+}
+
+
+/*******************************
+ * send HW message to hfc card *
+ *******************************/
+
+static void
+dsp_tone_hw_message(struct dsp *dsp, u8 *sample, int len)
+{
+	struct sk_buff *nskb;
+
+	/* unlocking is not required, because we don't expect a response */
+	nskb = _alloc_mISDN_skb(PH_CONTROL_REQ,
+		(len)?HFC_SPL_LOOP_ON:HFC_SPL_LOOP_OFF, len, sample,
+		GFP_ATOMIC);
+	if (nskb) {
+		if (dsp->ch.peer) {
+			if (dsp->ch.recv(dsp->ch.peer, nskb))
+				dev_kfree_skb(nskb);
+		} else
+			dev_kfree_skb(nskb);
+	}
+}
+
+
+/*****************
+ * timer expires *
+ *****************/
+void
+dsp_tone_timeout(void *arg)
+{
+	struct dsp *dsp = arg;
+	struct dsp_tone *tone = &dsp->tone;
+	struct pattern *pat = (struct pattern *)tone->pattern;
+	int index = tone->index;
+
+	if (!tone->tone)
+		return;
+
+	index++;
+	if (!pat->seq[index])
+		index = 0;
+	tone->index = index;
+
+	/* set next tone */
+	if (pat->data[index] == DATA_S)
+		dsp_tone_hw_message(dsp, 0, 0);
+	else
+		dsp_tone_hw_message(dsp, pat->data[index], *(pat->siz[index]));
+	/* set timer */
+	init_timer(&tone->tl);
+	tone->tl.expires = jiffies + (pat->seq[index] * HZ) / 8000;
+	add_timer(&tone->tl);
+}
+
+
+/********************
+ * set/release tone *
+ ********************/
+
+/*
+ * tones are relaized by streaming or by special loop commands if supported
+ * by hardware. when hardware is used, the patterns will be controlled by
+ * timers.
+ */
+int
+dsp_tone(struct dsp *dsp, int tone)
+{
+	struct pattern *pat;
+	int i;
+	struct dsp_tone *tonet = &dsp->tone;
+
+	tonet->software = 0;
+	tonet->hardware = 0;
+
+	/* we turn off the tone */
+	if (!tone) {
+		if (dsp->features.hfc_loops)
+		if (timer_pending(&tonet->tl))
+			del_timer(&tonet->tl);
+		if (dsp->features.hfc_loops)
+			dsp_tone_hw_message(dsp, NULL, 0);
+		tonet->tone = 0;
+		return 0;
+	}
+
+	pat = NULL;
+	i = 0;
+	while (pattern[i].tone) {
+		if (pattern[i].tone == tone) {
+			pat = &pattern[i];
+			break;
+		}
+		i++;
+	}
+	if (!pat) {
+		printk(KERN_WARNING "dsp: given tone 0x%x is invalid\n", tone);
+		return -EINVAL;
+	}
+	if (dsp_debug & DEBUG_DSP_TONE)
+		printk(KERN_DEBUG "%s: now starting tone %d (index=%d)\n",
+			__func__, tone, 0);
+	tonet->tone = tone;
+	tonet->pattern = pat;
+	tonet->index = 0;
+	tonet->count = 0;
+
+	if (dsp->features.hfc_loops) {
+		tonet->hardware = 1;
+		/* set first tone */
+		dsp_tone_hw_message(dsp, pat->data[0], *(pat->siz[0]));
+		/* set timer */
+		if (timer_pending(&tonet->tl))
+			del_timer(&tonet->tl);
+		init_timer(&tonet->tl);
+		tonet->tl.expires = jiffies + (pat->seq[0] * HZ) / 8000;
+		add_timer(&tonet->tl);
+	} else {
+		tonet->software = 1;
+	}
+
+	return 0;
+}
+
+
+
+
+
diff --git a/drivers/isdn/mISDN/fsm.c b/drivers/isdn/mISDN/fsm.c
new file mode 100644
index 00000000000..b5d6553f2dc
--- /dev/null
+++ b/drivers/isdn/mISDN/fsm.c
@@ -0,0 +1,183 @@
+/*
+ * finite state machine implementation
+ *
+ * Author       Karsten Keil <kkeil@novell.com>
+ *
+ * Thanks to    Jan den Ouden
+ *              Fritz Elfert
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include "fsm.h"
+
+#define FSM_TIMER_DEBUG 0
+
+void
+mISDN_FsmNew(struct Fsm *fsm,
+       struct FsmNode *fnlist, int fncount)
+{
+	int i;
+
+	fsm->jumpmatrix = kzalloc(sizeof(FSMFNPTR) * fsm->state_count *
+		fsm->event_count, GFP_KERNEL);
+
+	for (i = 0; i < fncount; i++)
+		if ((fnlist[i].state >= fsm->state_count) ||
+		    (fnlist[i].event >= fsm->event_count)) {
+			printk(KERN_ERR
+			    "mISDN_FsmNew Error: %d st(%ld/%ld) ev(%ld/%ld)\n",
+			    i, (long)fnlist[i].state, (long)fsm->state_count,
+			    (long)fnlist[i].event, (long)fsm->event_count);
+		} else
+			fsm->jumpmatrix[fsm->state_count * fnlist[i].event +
+			    fnlist[i].state] = (FSMFNPTR) fnlist[i].routine;
+}
+EXPORT_SYMBOL(mISDN_FsmNew);
+
+void
+mISDN_FsmFree(struct Fsm *fsm)
+{
+	kfree((void *) fsm->jumpmatrix);
+}
+EXPORT_SYMBOL(mISDN_FsmFree);
+
+int
+mISDN_FsmEvent(struct FsmInst *fi, int event, void *arg)
+{
+	FSMFNPTR r;
+
+	if ((fi->state >= fi->fsm->state_count) ||
+	    (event >= fi->fsm->event_count)) {
+		printk(KERN_ERR
+		    "mISDN_FsmEvent Error st(%ld/%ld) ev(%d/%ld)\n",
+		    (long)fi->state, (long)fi->fsm->state_count, event,
+		    (long)fi->fsm->event_count);
+		return 1;
+	}
+	r = fi->fsm->jumpmatrix[fi->fsm->state_count * event + fi->state];
+	if (r) {
+		if (fi->debug)
+			fi->printdebug(fi, "State %s Event %s",
+				fi->fsm->strState[fi->state],
+				fi->fsm->strEvent[event]);
+		r(fi, event, arg);
+		return 0;
+	} else {
+		if (fi->debug)
+			fi->printdebug(fi, "State %s Event %s no action",
+				fi->fsm->strState[fi->state],
+				fi->fsm->strEvent[event]);
+		return 1;
+	}
+}
+EXPORT_SYMBOL(mISDN_FsmEvent);
+
+void
+mISDN_FsmChangeState(struct FsmInst *fi, int newstate)
+{
+	fi->state = newstate;
+	if (fi->debug)
+		fi->printdebug(fi, "ChangeState %s",
+			fi->fsm->strState[newstate]);
+}
+EXPORT_SYMBOL(mISDN_FsmChangeState);
+
+static void
+FsmExpireTimer(struct FsmTimer *ft)
+{
+#if FSM_TIMER_DEBUG
+	if (ft->fi->debug)
+		ft->fi->printdebug(ft->fi, "FsmExpireTimer %lx", (long) ft);
+#endif
+	mISDN_FsmEvent(ft->fi, ft->event, ft->arg);
+}
+
+void
+mISDN_FsmInitTimer(struct FsmInst *fi, struct FsmTimer *ft)
+{
+	ft->fi = fi;
+	ft->tl.function = (void *) FsmExpireTimer;
+	ft->tl.data = (long) ft;
+#if FSM_TIMER_DEBUG
+	if (ft->fi->debug)
+		ft->fi->printdebug(ft->fi, "mISDN_FsmInitTimer %lx", (long) ft);
+#endif
+	init_timer(&ft->tl);
+}
+EXPORT_SYMBOL(mISDN_FsmInitTimer);
+
+void
+mISDN_FsmDelTimer(struct FsmTimer *ft, int where)
+{
+#if FSM_TIMER_DEBUG
+	if (ft->fi->debug)
+		ft->fi->printdebug(ft->fi, "mISDN_FsmDelTimer %lx %d",
+			(long) ft, where);
+#endif
+	del_timer(&ft->tl);
+}
+EXPORT_SYMBOL(mISDN_FsmDelTimer);
+
+int
+mISDN_FsmAddTimer(struct FsmTimer *ft,
+	    int millisec, int event, void *arg, int where)
+{
+
+#if FSM_TIMER_DEBUG
+	if (ft->fi->debug)
+		ft->fi->printdebug(ft->fi, "mISDN_FsmAddTimer %lx %d %d",
+			(long) ft, millisec, where);
+#endif
+
+	if (timer_pending(&ft->tl)) {
+		if (ft->fi->debug) {
+			printk(KERN_WARNING
+				"mISDN_FsmAddTimer: timer already active!\n");
+			ft->fi->printdebug(ft->fi,
+				"mISDN_FsmAddTimer already active!");
+		}
+		return -1;
+	}
+	init_timer(&ft->tl);
+	ft->event = event;
+	ft->arg = arg;
+	ft->tl.expires = jiffies + (millisec * HZ) / 1000;
+	add_timer(&ft->tl);
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_FsmAddTimer);
+
+void
+mISDN_FsmRestartTimer(struct FsmTimer *ft,
+	    int millisec, int event, void *arg, int where)
+{
+
+#if FSM_TIMER_DEBUG
+	if (ft->fi->debug)
+		ft->fi->printdebug(ft->fi, "mISDN_FsmRestartTimer %lx %d %d",
+			(long) ft, millisec, where);
+#endif
+
+	if (timer_pending(&ft->tl))
+		del_timer(&ft->tl);
+	init_timer(&ft->tl);
+	ft->event = event;
+	ft->arg = arg;
+	ft->tl.expires = jiffies + (millisec * HZ) / 1000;
+	add_timer(&ft->tl);
+}
+EXPORT_SYMBOL(mISDN_FsmRestartTimer);
diff --git a/drivers/isdn/mISDN/fsm.h b/drivers/isdn/mISDN/fsm.h
new file mode 100644
index 00000000000..928f5be192c
--- /dev/null
+++ b/drivers/isdn/mISDN/fsm.h
@@ -0,0 +1,67 @@
+/*
+ *
+ * Author       Karsten Keil <kkeil@novell.com>
+ *
+ * Thanks to    Jan den Ouden
+ *              Fritz Elfert
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _MISDN_FSM_H
+#define _MISDN_FSM_H
+
+#include <linux/timer.h>
+
+/* Statemachine */
+
+struct FsmInst;
+
+typedef void (*FSMFNPTR)(struct FsmInst *, int, void *);
+
+struct Fsm {
+	FSMFNPTR *jumpmatrix;
+	int state_count, event_count;
+	char **strEvent, **strState;
+};
+
+struct FsmInst {
+	struct Fsm *fsm;
+	int state;
+	int debug;
+	void *userdata;
+	int userint;
+	void (*printdebug) (struct FsmInst *, char *, ...);
+};
+
+struct FsmNode {
+	int state, event;
+	void (*routine) (struct FsmInst *, int, void *);
+};
+
+struct FsmTimer {
+	struct FsmInst *fi;
+	struct timer_list tl;
+	int event;
+	void *arg;
+};
+
+extern void mISDN_FsmNew(struct Fsm *, struct FsmNode *, int);
+extern void mISDN_FsmFree(struct Fsm *);
+extern int mISDN_FsmEvent(struct FsmInst *, int , void *);
+extern void mISDN_FsmChangeState(struct FsmInst *, int);
+extern void mISDN_FsmInitTimer(struct FsmInst *, struct FsmTimer *);
+extern int mISDN_FsmAddTimer(struct FsmTimer *, int, int, void *, int);
+extern void mISDN_FsmRestartTimer(struct FsmTimer *, int, int, void *, int);
+extern void mISDN_FsmDelTimer(struct FsmTimer *, int);
+
+#endif
diff --git a/drivers/isdn/mISDN/hwchannel.c b/drivers/isdn/mISDN/hwchannel.c
new file mode 100644
index 00000000000..2596fba4e61
--- /dev/null
+++ b/drivers/isdn/mISDN/hwchannel.c
@@ -0,0 +1,365 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/mISDNhw.h>
+
+static void
+dchannel_bh(struct work_struct *ws)
+{
+	struct dchannel	*dch  = container_of(ws, struct dchannel, workq);
+	struct sk_buff	*skb;
+	int		err;
+
+	if (test_and_clear_bit(FLG_RECVQUEUE, &dch->Flags)) {
+		while ((skb = skb_dequeue(&dch->rqueue))) {
+			if (likely(dch->dev.D.peer)) {
+				err = dch->dev.D.recv(dch->dev.D.peer, skb);
+				if (err)
+					dev_kfree_skb(skb);
+			} else
+				dev_kfree_skb(skb);
+		}
+	}
+	if (test_and_clear_bit(FLG_PHCHANGE, &dch->Flags)) {
+		if (dch->phfunc)
+			dch->phfunc(dch);
+	}
+}
+
+static void
+bchannel_bh(struct work_struct *ws)
+{
+	struct bchannel	*bch  = container_of(ws, struct bchannel, workq);
+	struct sk_buff	*skb;
+	int		err;
+
+	if (test_and_clear_bit(FLG_RECVQUEUE, &bch->Flags)) {
+		while ((skb = skb_dequeue(&bch->rqueue))) {
+			if (bch->rcount >= 64)
+				printk(KERN_WARNING "B-channel %p receive "
+					"queue if full, but empties...\n", bch);
+			bch->rcount--;
+			if (likely(bch->ch.peer)) {
+				err = bch->ch.recv(bch->ch.peer, skb);
+				if (err)
+					dev_kfree_skb(skb);
+			} else
+				dev_kfree_skb(skb);
+		}
+	}
+}
+
+int
+mISDN_initdchannel(struct dchannel *ch, int maxlen, void *phf)
+{
+	test_and_set_bit(FLG_HDLC, &ch->Flags);
+	ch->maxlen = maxlen;
+	ch->hw = NULL;
+	ch->rx_skb = NULL;
+	ch->tx_skb = NULL;
+	ch->tx_idx = 0;
+	ch->phfunc = phf;
+	skb_queue_head_init(&ch->squeue);
+	skb_queue_head_init(&ch->rqueue);
+	INIT_LIST_HEAD(&ch->dev.bchannels);
+	INIT_WORK(&ch->workq, dchannel_bh);
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_initdchannel);
+
+int
+mISDN_initbchannel(struct bchannel *ch, int maxlen)
+{
+	ch->Flags = 0;
+	ch->maxlen = maxlen;
+	ch->hw = NULL;
+	ch->rx_skb = NULL;
+	ch->tx_skb = NULL;
+	ch->tx_idx = 0;
+	skb_queue_head_init(&ch->rqueue);
+	ch->rcount = 0;
+	ch->next_skb = NULL;
+	INIT_WORK(&ch->workq, bchannel_bh);
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_initbchannel);
+
+int
+mISDN_freedchannel(struct dchannel *ch)
+{
+	if (ch->tx_skb) {
+		dev_kfree_skb(ch->tx_skb);
+		ch->tx_skb = NULL;
+	}
+	if (ch->rx_skb) {
+		dev_kfree_skb(ch->rx_skb);
+		ch->rx_skb = NULL;
+	}
+	skb_queue_purge(&ch->squeue);
+	skb_queue_purge(&ch->rqueue);
+	flush_scheduled_work();
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_freedchannel);
+
+int
+mISDN_freebchannel(struct bchannel *ch)
+{
+	if (ch->tx_skb) {
+		dev_kfree_skb(ch->tx_skb);
+		ch->tx_skb = NULL;
+	}
+	if (ch->rx_skb) {
+		dev_kfree_skb(ch->rx_skb);
+		ch->rx_skb = NULL;
+	}
+	if (ch->next_skb) {
+		dev_kfree_skb(ch->next_skb);
+		ch->next_skb = NULL;
+	}
+	skb_queue_purge(&ch->rqueue);
+	ch->rcount = 0;
+	flush_scheduled_work();
+	return 0;
+}
+EXPORT_SYMBOL(mISDN_freebchannel);
+
+static inline u_int
+get_sapi_tei(u_char *p)
+{
+	u_int	sapi, tei;
+
+	sapi = *p >> 2;
+	tei = p[1] >> 1;
+	return sapi | (tei << 8);
+}
+
+void
+recv_Dchannel(struct dchannel *dch)
+{
+	struct mISDNhead *hh;
+
+	if (dch->rx_skb->len < 2) { /* at least 2 for sapi / tei */
+		dev_kfree_skb(dch->rx_skb);
+		dch->rx_skb = NULL;
+		return;
+	}
+	hh = mISDN_HEAD_P(dch->rx_skb);
+	hh->prim = PH_DATA_IND;
+	hh->id = get_sapi_tei(dch->rx_skb->data);
+	skb_queue_tail(&dch->rqueue, dch->rx_skb);
+	dch->rx_skb = NULL;
+	schedule_event(dch, FLG_RECVQUEUE);
+}
+EXPORT_SYMBOL(recv_Dchannel);
+
+void
+recv_Bchannel(struct bchannel *bch)
+{
+	struct mISDNhead *hh;
+
+	hh = mISDN_HEAD_P(bch->rx_skb);
+	hh->prim = PH_DATA_IND;
+	hh->id = MISDN_ID_ANY;
+	if (bch->rcount >= 64) {
+		dev_kfree_skb(bch->rx_skb);
+		bch->rx_skb = NULL;
+		return;
+	}
+	bch->rcount++;
+	skb_queue_tail(&bch->rqueue, bch->rx_skb);
+	bch->rx_skb = NULL;
+	schedule_event(bch, FLG_RECVQUEUE);
+}
+EXPORT_SYMBOL(recv_Bchannel);
+
+void
+recv_Dchannel_skb(struct dchannel *dch, struct sk_buff *skb)
+{
+	skb_queue_tail(&dch->rqueue, skb);
+	schedule_event(dch, FLG_RECVQUEUE);
+}
+EXPORT_SYMBOL(recv_Dchannel_skb);
+
+void
+recv_Bchannel_skb(struct bchannel *bch, struct sk_buff *skb)
+{
+	if (bch->rcount >= 64) {
+		dev_kfree_skb(skb);
+		return;
+	}
+	bch->rcount++;
+	skb_queue_tail(&bch->rqueue, skb);
+	schedule_event(bch, FLG_RECVQUEUE);
+}
+EXPORT_SYMBOL(recv_Bchannel_skb);
+
+static void
+confirm_Dsend(struct dchannel *dch)
+{
+	struct sk_buff	*skb;
+
+	skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(dch->tx_skb),
+	    0, NULL, GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_ERR "%s: no skb id %x\n", __func__,
+		    mISDN_HEAD_ID(dch->tx_skb));
+		return;
+	}
+	skb_queue_tail(&dch->rqueue, skb);
+	schedule_event(dch, FLG_RECVQUEUE);
+}
+
+int
+get_next_dframe(struct dchannel *dch)
+{
+	dch->tx_idx = 0;
+	dch->tx_skb = skb_dequeue(&dch->squeue);
+	if (dch->tx_skb) {
+		confirm_Dsend(dch);
+		return 1;
+	}
+	dch->tx_skb = NULL;
+	test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
+	return 0;
+}
+EXPORT_SYMBOL(get_next_dframe);
+
+void
+confirm_Bsend(struct bchannel *bch)
+{
+	struct sk_buff	*skb;
+
+	if (bch->rcount >= 64)
+		return;
+	skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(bch->tx_skb),
+	    0, NULL, GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_ERR "%s: no skb id %x\n", __func__,
+		    mISDN_HEAD_ID(bch->tx_skb));
+		return;
+	}
+	bch->rcount++;
+	skb_queue_tail(&bch->rqueue, skb);
+	schedule_event(bch, FLG_RECVQUEUE);
+}
+EXPORT_SYMBOL(confirm_Bsend);
+
+int
+get_next_bframe(struct bchannel *bch)
+{
+	bch->tx_idx = 0;
+	if (test_bit(FLG_TX_NEXT, &bch->Flags)) {
+		bch->tx_skb = bch->next_skb;
+		if (bch->tx_skb) {
+			bch->next_skb = NULL;
+			test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
+			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
+				confirm_Bsend(bch); /* not for transparent */
+			return 1;
+		} else {
+			test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
+			printk(KERN_WARNING "B TX_NEXT without skb\n");
+		}
+	}
+	bch->tx_skb = NULL;
+	test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
+	return 0;
+}
+EXPORT_SYMBOL(get_next_bframe);
+
+void
+queue_ch_frame(struct mISDNchannel *ch, u_int pr, int id, struct sk_buff *skb)
+{
+	struct mISDNhead *hh;
+
+	if (!skb) {
+		_queue_data(ch, pr, id, 0, NULL, GFP_ATOMIC);
+	} else {
+		if (ch->peer) {
+			hh = mISDN_HEAD_P(skb);
+			hh->prim = pr;
+			hh->id = id;
+			if (!ch->recv(ch->peer, skb))
+				return;
+		}
+		dev_kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL(queue_ch_frame);
+
+int
+dchannel_senddata(struct dchannel *ch, struct sk_buff *skb)
+{
+	/* check oversize */
+	if (skb->len <= 0) {
+		printk(KERN_WARNING "%s: skb too small\n", __func__);
+		return -EINVAL;
+	}
+	if (skb->len > ch->maxlen) {
+		printk(KERN_WARNING "%s: skb too large(%d/%d)\n",
+			__func__, skb->len, ch->maxlen);
+		return -EINVAL;
+	}
+	/* HW lock must be obtained */
+	if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) {
+		skb_queue_tail(&ch->squeue, skb);
+		return 0;
+	} else {
+		/* write to fifo */
+		ch->tx_skb = skb;
+		ch->tx_idx = 0;
+		return 1;
+	}
+}
+EXPORT_SYMBOL(dchannel_senddata);
+
+int
+bchannel_senddata(struct bchannel *ch, struct sk_buff *skb)
+{
+
+	/* check oversize */
+	if (skb->len <= 0) {
+		printk(KERN_WARNING "%s: skb too small\n", __func__);
+		return -EINVAL;
+	}
+	if (skb->len > ch->maxlen) {
+		printk(KERN_WARNING "%s: skb too large(%d/%d)\n",
+			__func__, skb->len, ch->maxlen);
+		return -EINVAL;
+	}
+	/* HW lock must be obtained */
+	/* check for pending next_skb */
+	if (ch->next_skb) {
+		printk(KERN_WARNING
+		    "%s: next_skb exist ERROR (skb->len=%d next_skb->len=%d)\n",
+		    __func__, skb->len, ch->next_skb->len);
+		return -EBUSY;
+	}
+	if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) {
+		test_and_set_bit(FLG_TX_NEXT, &ch->Flags);
+		ch->next_skb = skb;
+		return 0;
+	} else {
+		/* write to fifo */
+		ch->tx_skb = skb;
+		ch->tx_idx = 0;
+		return 1;
+	}
+}
+EXPORT_SYMBOL(bchannel_senddata);
diff --git a/drivers/isdn/mISDN/l1oip.h b/drivers/isdn/mISDN/l1oip.h
new file mode 100644
index 00000000000..a23d575449f
--- /dev/null
+++ b/drivers/isdn/mISDN/l1oip.h
@@ -0,0 +1,91 @@
+/*
+ * see notice in l1oip.c
+ */
+
+/* debugging */
+#define DEBUG_L1OIP_INIT	0x00010000
+#define DEBUG_L1OIP_SOCKET	0x00020000
+#define DEBUG_L1OIP_MGR		0x00040000
+#define DEBUG_L1OIP_MSG		0x00080000
+
+/* enable to disorder received bchannels by sequence 2143658798... */
+/*
+#define REORDER_DEBUG
+*/
+
+/* frames */
+#define L1OIP_MAX_LEN		2048		/* max packet size form l2 */
+#define L1OIP_MAX_PERFRAME	1400		/* max data size in one frame */
+
+
+/* timers */
+#define L1OIP_KEEPALIVE		15
+#define L1OIP_TIMEOUT		65
+
+
+/* socket */
+#define L1OIP_DEFAULTPORT	931
+
+
+/* channel structure */
+struct l1oip_chan {
+	struct dchannel       	*dch;
+	struct bchannel       	*bch;
+	u32			tx_counter;	/* counts xmit bytes/packets */
+	u32			rx_counter;	/* counts recv bytes/packets */
+	u32			codecstate;	/* used by codec to save data */
+#ifdef REORDER_DEBUG
+	int			disorder_flag;
+	struct sk_buff		*disorder_skb;
+	u32			disorder_cnt;
+#endif
+};
+
+
+/* card structure */
+struct l1oip {
+	struct list_head        list;
+
+	/* card */
+	int			registered;	/* if registered with mISDN */
+	char			name[MISDN_MAX_IDLEN];
+	int			idx;		/* card index */
+	int			pri;		/* 1=pri, 0=bri */
+	int			d_idx;		/* current dchannel number */
+	int			b_num;		/* number of bchannels */
+	u32			id;		/* id of connection */
+	int			ondemand;	/* if transmis. is on demand */
+	int			bundle;		/* bundle channels in one frm */
+	int			codec;		/* codec to use for transmis. */
+	int			limit;		/* limit number of bchannels */
+
+	/* timer */
+	struct timer_list 	keep_tl;
+	struct timer_list 	timeout_tl;
+	int			timeout_on;
+	struct work_struct	workq;
+
+	/* socket */
+	struct socket 		*socket;	/* if set, socket is created */
+	struct completion 	socket_complete;/* completion of sock thread */
+	struct task_struct	*socket_thread;
+	spinlock_t 		socket_lock;	/* access sock outside thread */
+	u32			remoteip;	/* if all set, ip is assigned */
+	u16	 		localport;	/* must always be set */
+	u16	 		remoteport;	/* must always be set */
+	struct sockaddr_in	sin_local;	/* local socket name */
+	struct sockaddr_in	sin_remote;	/* remote socket name */
+	struct msghdr		sendmsg;	/* ip message to send */
+	struct iovec		sendiov;	/* iov for message */
+
+	/* frame */
+	struct l1oip_chan	chan[128];	/* channel instances */
+};
+
+extern int l1oip_law_to_4bit(u8 *data, int len, u8 *result, u32 *state);
+extern int l1oip_4bit_to_law(u8 *data, int len, u8 *result);
+extern int l1oip_alaw_to_ulaw(u8 *data, int len, u8 *result);
+extern int l1oip_ulaw_to_alaw(u8 *data, int len, u8 *result);
+extern void l1oip_4bit_free(void);
+extern int l1oip_4bit_alloc(int ulaw);
+
diff --git a/drivers/isdn/mISDN/l1oip_codec.c b/drivers/isdn/mISDN/l1oip_codec.c
new file mode 100644
index 00000000000..a2dc4570ef4
--- /dev/null
+++ b/drivers/isdn/mISDN/l1oip_codec.c
@@ -0,0 +1,374 @@
+/*
+
+ * l1oip_codec.c  generic codec using lookup table
+ *  -> conversion from a-Law to u-Law
+ *  -> conversion from u-Law to a-Law
+ *  -> compression by reducing the number of sample resolution to 4
+ *
+ * NOTE: It is not compatible with any standard codec like ADPCM.
+ *
+ * Author	Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This program is free software; 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.
+
+ */
+
+/*
+
+How the codec works:
+--------------------
+
+The volume is increased to increase the dynamic range of the audio signal.
+Each sample is converted to a-LAW with only 16 steps of level resolution.
+A pair of two samples are stored in one byte.
+
+The first byte is stored in the upper bits, the second byte is stored in the
+lower bits.
+
+To speed up compression and decompression, two lookup tables are formed:
+
+- 16 bits index for two samples (law encoded) with 8 bit compressed result.
+- 8 bits index for one compressed data with 16 bits decompressed result.
+
+NOTE: The bytes are handled as they are law-encoded.
+
+*/
+
+#include <linux/vmalloc.h>
+#include <linux/mISDNif.h>
+#include "core.h"
+
+/* definitions of codec. don't use calculations, code may run slower. */
+
+static u8 *table_com;
+static u16 *table_dec;
+
+
+/* alaw -> ulaw */
+static u8 alaw_to_ulaw[256] =
+{
+	0xab, 0x2b, 0xe3, 0x63, 0x8b, 0x0b, 0xc9, 0x49,
+	0xba, 0x3a, 0xf6, 0x76, 0x9b, 0x1b, 0xd7, 0x57,
+	0xa3, 0x23, 0xdd, 0x5d, 0x83, 0x03, 0xc1, 0x41,
+	0xb2, 0x32, 0xeb, 0x6b, 0x93, 0x13, 0xcf, 0x4f,
+	0xaf, 0x2f, 0xe7, 0x67, 0x8f, 0x0f, 0xcd, 0x4d,
+	0xbe, 0x3e, 0xfe, 0x7e, 0x9f, 0x1f, 0xdb, 0x5b,
+	0xa7, 0x27, 0xdf, 0x5f, 0x87, 0x07, 0xc5, 0x45,
+	0xb6, 0x36, 0xef, 0x6f, 0x97, 0x17, 0xd3, 0x53,
+	0xa9, 0x29, 0xe1, 0x61, 0x89, 0x09, 0xc7, 0x47,
+	0xb8, 0x38, 0xf2, 0x72, 0x99, 0x19, 0xd5, 0x55,
+	0xa1, 0x21, 0xdc, 0x5c, 0x81, 0x01, 0xbf, 0x3f,
+	0xb0, 0x30, 0xe9, 0x69, 0x91, 0x11, 0xce, 0x4e,
+	0xad, 0x2d, 0xe5, 0x65, 0x8d, 0x0d, 0xcb, 0x4b,
+	0xbc, 0x3c, 0xfa, 0x7a, 0x9d, 0x1d, 0xd9, 0x59,
+	0xa5, 0x25, 0xde, 0x5e, 0x85, 0x05, 0xc3, 0x43,
+	0xb4, 0x34, 0xed, 0x6d, 0x95, 0x15, 0xd1, 0x51,
+	0xac, 0x2c, 0xe4, 0x64, 0x8c, 0x0c, 0xca, 0x4a,
+	0xbb, 0x3b, 0xf8, 0x78, 0x9c, 0x1c, 0xd8, 0x58,
+	0xa4, 0x24, 0xde, 0x5e, 0x84, 0x04, 0xc2, 0x42,
+	0xb3, 0x33, 0xec, 0x6c, 0x94, 0x14, 0xd0, 0x50,
+	0xb0, 0x30, 0xe8, 0x68, 0x90, 0x10, 0xce, 0x4e,
+	0xbf, 0x3f, 0xfe, 0x7e, 0xa0, 0x20, 0xdc, 0x5c,
+	0xa8, 0x28, 0xe0, 0x60, 0x88, 0x08, 0xc6, 0x46,
+	0xb7, 0x37, 0xf0, 0x70, 0x98, 0x18, 0xd4, 0x54,
+	0xaa, 0x2a, 0xe2, 0x62, 0x8a, 0x0a, 0xc8, 0x48,
+	0xb9, 0x39, 0xf4, 0x74, 0x9a, 0x1a, 0xd6, 0x56,
+	0xa2, 0x22, 0xdd, 0x5d, 0x82, 0x02, 0xc0, 0x40,
+	0xb1, 0x31, 0xea, 0x6a, 0x92, 0x12, 0xcf, 0x4f,
+	0xae, 0x2e, 0xe6, 0x66, 0x8e, 0x0e, 0xcc, 0x4c,
+	0xbd, 0x3d, 0xfc, 0x7c, 0x9e, 0x1e, 0xda, 0x5a,
+	0xa6, 0x26, 0xdf, 0x5f, 0x86, 0x06, 0xc4, 0x44,
+	0xb5, 0x35, 0xee, 0x6e, 0x96, 0x16, 0xd2, 0x52
+};
+
+/* ulaw -> alaw */
+static u8 ulaw_to_alaw[256] =
+{
+	0xab, 0x55, 0xd5, 0x15, 0x95, 0x75, 0xf5, 0x35,
+	0xb5, 0x45, 0xc5, 0x05, 0x85, 0x65, 0xe5, 0x25,
+	0xa5, 0x5d, 0xdd, 0x1d, 0x9d, 0x7d, 0xfd, 0x3d,
+	0xbd, 0x4d, 0xcd, 0x0d, 0x8d, 0x6d, 0xed, 0x2d,
+	0xad, 0x51, 0xd1, 0x11, 0x91, 0x71, 0xf1, 0x31,
+	0xb1, 0x41, 0xc1, 0x01, 0x81, 0x61, 0xe1, 0x21,
+	0x59, 0xd9, 0x19, 0x99, 0x79, 0xf9, 0x39, 0xb9,
+	0x49, 0xc9, 0x09, 0x89, 0x69, 0xe9, 0x29, 0xa9,
+	0xd7, 0x17, 0x97, 0x77, 0xf7, 0x37, 0xb7, 0x47,
+	0xc7, 0x07, 0x87, 0x67, 0xe7, 0x27, 0xa7, 0xdf,
+	0x9f, 0x7f, 0xff, 0x3f, 0xbf, 0x4f, 0xcf, 0x0f,
+	0x8f, 0x6f, 0xef, 0x2f, 0x53, 0x13, 0x73, 0x33,
+	0xb3, 0x43, 0xc3, 0x03, 0x83, 0x63, 0xe3, 0x23,
+	0xa3, 0x5b, 0xdb, 0x1b, 0x9b, 0x7b, 0xfb, 0x3b,
+	0xbb, 0xbb, 0x4b, 0x4b, 0xcb, 0xcb, 0x0b, 0x0b,
+	0x8b, 0x8b, 0x6b, 0x6b, 0xeb, 0xeb, 0x2b, 0x2b,
+	0xab, 0x54, 0xd4, 0x14, 0x94, 0x74, 0xf4, 0x34,
+	0xb4, 0x44, 0xc4, 0x04, 0x84, 0x64, 0xe4, 0x24,
+	0xa4, 0x5c, 0xdc, 0x1c, 0x9c, 0x7c, 0xfc, 0x3c,
+	0xbc, 0x4c, 0xcc, 0x0c, 0x8c, 0x6c, 0xec, 0x2c,
+	0xac, 0x50, 0xd0, 0x10, 0x90, 0x70, 0xf0, 0x30,
+	0xb0, 0x40, 0xc0, 0x00, 0x80, 0x60, 0xe0, 0x20,
+	0x58, 0xd8, 0x18, 0x98, 0x78, 0xf8, 0x38, 0xb8,
+	0x48, 0xc8, 0x08, 0x88, 0x68, 0xe8, 0x28, 0xa8,
+	0xd6, 0x16, 0x96, 0x76, 0xf6, 0x36, 0xb6, 0x46,
+	0xc6, 0x06, 0x86, 0x66, 0xe6, 0x26, 0xa6, 0xde,
+	0x9e, 0x7e, 0xfe, 0x3e, 0xbe, 0x4e, 0xce, 0x0e,
+	0x8e, 0x6e, 0xee, 0x2e, 0x52, 0x12, 0x72, 0x32,
+	0xb2, 0x42, 0xc2, 0x02, 0x82, 0x62, 0xe2, 0x22,
+	0xa2, 0x5a, 0xda, 0x1a, 0x9a, 0x7a, 0xfa, 0x3a,
+	0xba, 0xba, 0x4a, 0x4a, 0xca, 0xca, 0x0a, 0x0a,
+	0x8a, 0x8a, 0x6a, 0x6a, 0xea, 0xea, 0x2a, 0x2a
+};
+
+/* alaw -> 4bit compression */
+static u8 alaw_to_4bit[256] = {
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0d, 0x02,
+	0x0e, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x01, 0x0a, 0x05,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04,
+	0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03,
+	0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04,
+};
+
+/* 4bit -> alaw decompression */
+static u8 _4bit_to_alaw[16] = {
+	0x5d, 0x51, 0xd9, 0xd7, 0x5f, 0x53, 0xa3, 0x4b,
+	0x2a, 0x3a, 0x22, 0x2e, 0x26, 0x56, 0x20, 0x2c,
+};
+
+/* ulaw -> 4bit compression */
+static u8 ulaw_to_4bit[256] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02,
+	0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02,
+	0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
+	0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x04, 0x04,
+	0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
+	0x04, 0x04, 0x04, 0x04, 0x05, 0x05, 0x05, 0x05,
+	0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
+	0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
+	0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x08,
+	0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,
+	0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,
+	0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,
+	0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,
+	0x0f, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e,
+	0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e,
+	0x0e, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d,
+	0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0b, 0x0b,
+	0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b,
+	0x0b, 0x0b, 0x0b, 0x0b, 0x0a, 0x0a, 0x0a, 0x0a,
+	0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a,
+	0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09,
+	0x09, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+	0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+};
+
+/* 4bit -> ulaw decompression */
+static u8 _4bit_to_ulaw[16] = {
+	0x11, 0x21, 0x31, 0x40, 0x4e, 0x5c, 0x68, 0x71,
+	0xfe, 0xef, 0xe7, 0xdb, 0xcd, 0xbf, 0xaf, 0x9f,
+};
+
+
+/*
+ * Compresses data to the result buffer
+ * The result size must be at least half of the input buffer.
+ * The number of samples also must be even!
+ */
+int
+l1oip_law_to_4bit(u8 *data, int len, u8 *result, u32 *state)
+{
+	int ii, i = 0, o = 0;
+
+	if (!len)
+		return 0;
+
+	/* send saved byte and first input byte */
+	if (*state) {
+		*result++ = table_com[(((*state)<<8)&0xff00) | (*data++)];
+		len--;
+		o++;
+	}
+
+	ii = len >> 1;
+
+	while (i < ii) {
+		*result++ = table_com[(data[0]<<8) | (data[1])];
+		data += 2;
+		i++;
+		o++;
+	}
+
+	/* if len has an odd number, we save byte for next call */
+	if (len & 1)
+		*state = 0x100 + *data;
+	else
+		*state = 0;
+
+	return o;
+}
+
+/* Decompress data to the result buffer
+ * The result size must be the number of sample in packet. (2 * input data)
+ * The number of samples in the result are even!
+ */
+int
+l1oip_4bit_to_law(u8 *data, int len, u8 *result)
+{
+	int i = 0;
+	u16 r;
+
+	while (i < len) {
+		r = table_dec[*data++];
+		*result++ = r>>8;
+		*result++ = r;
+		i++;
+	}
+
+	return len << 1;
+}
+
+
+/*
+ * law conversion
+ */
+int
+l1oip_alaw_to_ulaw(u8 *data, int len, u8 *result)
+{
+	int i = 0;
+
+	while (i < len) {
+		*result++ = alaw_to_ulaw[*data++];
+		i++;
+	}
+
+	return len;
+}
+
+int
+l1oip_ulaw_to_alaw(u8 *data, int len, u8 *result)
+{
+	int i = 0;
+
+	while (i < len) {
+		*result++ = ulaw_to_alaw[*data++];
+		i++;
+	}
+
+	return len;
+}
+
+
+/*
+ * generate/free compression and decompression table
+ */
+void
+l1oip_4bit_free(void)
+{
+	if (table_dec)
+		vfree(table_dec);
+	if (table_com)
+		vfree(table_com);
+	table_com = NULL;
+	table_dec = NULL;
+}
+
+int
+l1oip_4bit_alloc(int ulaw)
+{
+	int i1, i2, c, sample;
+
+	/* in case, it is called again */
+	if (table_dec)
+		return 0;
+
+	/* alloc conversion tables */
+	table_com = vmalloc(65536);
+	table_dec = vmalloc(512);
+	if (!table_com | !table_dec) {
+		l1oip_4bit_free();
+		return -ENOMEM;
+	}
+	memset(table_com, 0, 65536);
+	memset(table_dec, 0, 512);
+	/* generate compression table */
+	i1 = 0;
+	while (i1 < 256) {
+		if (ulaw)
+			c = ulaw_to_4bit[i1];
+		else
+			c = alaw_to_4bit[i1];
+		i2 = 0;
+		while (i2 < 256) {
+			table_com[(i1<<8) | i2] |= (c<<4);
+			table_com[(i2<<8) | i1] |= c;
+			i2++;
+		}
+		i1++;
+	}
+
+	/* generate decompression table */
+	i1 = 0;
+	while (i1 < 16) {
+		if (ulaw)
+			sample = _4bit_to_ulaw[i1];
+		else
+			sample = _4bit_to_alaw[i1];
+		i2 = 0;
+		while (i2 < 16) {
+			table_dec[(i1<<4) | i2] |= (sample<<8);
+			table_dec[(i2<<4) | i1] |= sample;
+			i2++;
+		}
+		i1++;
+	}
+
+	return 0;
+}
+
+
diff --git a/drivers/isdn/mISDN/l1oip_core.c b/drivers/isdn/mISDN/l1oip_core.c
new file mode 100644
index 00000000000..155b99780c4
--- /dev/null
+++ b/drivers/isdn/mISDN/l1oip_core.c
@@ -0,0 +1,1518 @@
+/*
+
+ * l1oip.c  low level driver for tunneling layer 1 over IP
+ *
+ * NOTE: It is not compatible with TDMoIP nor "ISDN over IP".
+ *
+ * Author	Andreas Eversberg (jolly@eversberg.eu)
+ *
+ * This program is free software; 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.
+ *
+ */
+
+/* module parameters:
+ * type:
+	Value 1	= BRI
+	Value 2	= PRI
+	Value 3 = BRI (multi channel frame, not supported yet)
+	Value 4 = PRI (multi channel frame, not supported yet)
+	A multi channel frame reduces overhead to a single frame for all
+	b-channels, but increases delay.
+	(NOTE: Multi channel frames are not implemented yet.)
+
+ * codec:
+	Value 0 = transparent (default)
+	Value 1 = transfer ALAW
+	Value 2 = transfer ULAW
+	Value 3 = transfer generic 4 bit compression.
+
+ * ulaw:
+	0 = we use a-Law (default)
+	1 = we use u-Law
+
+ * limit:
+	limitation of B-channels to control bandwidth (1...126)
+	BRI: 1 or 2
+	PRI: 1-30, 31-126 (126, because dchannel ist not counted here)
+	Also limited ressources are used for stack, resulting in less channels.
+	It is possible to have more channels than 30 in PRI mode, this must
+	be supported by the application.
+
+ * ip:
+	byte representation of remote ip address (127.0.0.1 -> 127,0,0,1)
+	If not given or four 0, no remote address is set.
+	For multiple interfaces, concat ip addresses. (127,0,0,1,127,0,0,1)
+
+ * port:
+	port number (local interface)
+	If not given or 0, port 931 is used for fist instance, 932 for next...
+	For multiple interfaces, different ports must be given.
+
+ * remoteport:
+	port number (remote interface)
+	If not given or 0, remote port equals local port
+	For multiple interfaces on equal sites, different ports must be given.
+
+ * ondemand:
+	0 = fixed (always transmit packets, even when remote side timed out)
+	1 = on demand (only transmit packets, when remote side is detected)
+	the default is 0
+	NOTE: ID must also be set for on demand.
+
+ * id:
+	optional value to identify frames. This value must be equal on both
+	peers and should be random. If omitted or 0, no ID is transmitted.
+
+ * debug:
+	NOTE: only one debug value must be given for all cards
+	enable debugging (see l1oip.h for debug options)
+
+
+Special mISDN controls:
+
+ op = MISDN_CTRL_SETPEER*
+ p1 = bytes 0-3 : remote IP address in network order (left element first)
+ p2 = bytes 1-2 : remote port in network order (high byte first)
+ optional:
+ p2 = bytes 3-4 : local port in network order (high byte first)
+
+ op = MISDN_CTRL_UNSETPEER*
+
+ * Use l1oipctrl for comfortable setting or removing ip address.
+   (Layer 1 Over IP CTRL)
+
+
+L1oIP-Protocol
+--------------
+
+Frame Header:
+
+ 7 6 5 4 3 2 1 0
++---------------+
+|Ver|T|I|Coding |
++---------------+
+|  ID byte 3 *  |
++---------------+
+|  ID byte 2 *  |
++---------------+
+|  ID byte 1 *  |
++---------------+
+|  ID byte 0 *  |
++---------------+
+|M|   Channel   |
++---------------+
+|    Length *   |
++---------------+
+| Time Base MSB |
++---------------+
+| Time Base LSB |
++---------------+
+| Data....	|
+
+...
+
+|               |
++---------------+
+|M|   Channel   |
++---------------+
+|    Length *   |
++---------------+
+| Time Base MSB |
++---------------+
+| Time Base LSB |
++---------------+
+| Data....	|
+
+...
+
+
+* Only included in some cases.
+
+- Ver = Version
+If version is missmatch, the frame must be ignored.
+
+- T = Type of interface
+Must be 0 for S0 or 1 for E1.
+
+- I = Id present
+If bit is set, four ID bytes are included in frame.
+
+- ID = Connection ID
+Additional ID to prevent Denial of Service attacs. Also it prevents hijacking
+connections with dynamic IP. The ID should be random and must not be 0.
+
+- Coding = Type of codec
+Must be 0 for no transcoding. Also for D-channel and other HDLC frames.
+ 1 and 2 are reserved for explicitly use of a-LAW or u-LAW codec.
+ 3 is used for generic table compressor.
+
+- M = More channels to come. If this flag is 1, the following byte contains
+the length of the channel data. After the data block, the next channel will
+be defined. The flag for the last channel block (or if only one channel is
+transmitted), must be 0 and no length is given.
+
+- Channel = Channel number
+0 reserved
+1-3 channel data for S0 (3 is D-channel)
+1-31 channel data for E1 (16 is D-channel)
+32-127 channel data for extended E1 (16 is D-channel)
+
+- The length is used if the M-flag is 1. It is used to find the next channel
+inside frame.
+NOTE: A value of 0 equals 256 bytes of data.
+ -> For larger data blocks, a single frame must be used.
+ -> For larger streams, a single frame or multiple blocks with same channel ID
+   must be used.
+
+- Time Base = Timestamp of first sample in frame
+The "Time Base" is used to rearange packets and to detect packet loss.
+The 16 bits are sent in network order (MSB first) and count 1/8000 th of a
+second. This causes a wrap arround each 8,192 seconds. There is no requirement
+for the initial "Time Base", but 0 should be used for the first packet.
+In case of HDLC data, this timestamp counts the packet or byte number.
+
+
+Two Timers:
+
+After initialisation, a timer of 15 seconds is started. Whenever a packet is
+transmitted, the timer is reset to 15 seconds again. If the timer expires, an
+empty packet is transmitted. This keep the connection alive.
+
+When a valid packet is received, a timer 65 seconds is started. The interface
+become ACTIVE. If the timer expires, the interface becomes INACTIVE.
+
+
+Dynamic IP handling:
+
+To allow dynamic IP, the ID must be non 0. In this case, any packet with the
+correct port number and ID will be accepted. If the remote side changes its IP
+the new IP is used for all transmitted packets until it changes again.
+
+
+On Demand:
+
+If the ondemand parameter is given, the remote IP is set to 0 on timeout.
+This will stop keepalive traffic to remote. If the remote is online again,
+traffic will continue to the remote address. This is usefull for road warriors.
+This feature only works with ID set, otherwhise it is highly unsecure.
+
+
+Socket and Thread
+-----------------
+
+The complete socket opening and closing is done by a thread.
+When the thread opened a socket, the hc->socket descriptor is set. Whenever a
+packet shall be sent to the socket, the hc->socket must be checked wheter not
+NULL. To prevent change in socket descriptor, the hc->socket_lock must be used.
+To change the socket, a recall of l1oip_socket_open() will safely kill the
+socket process and create a new one.
+
+*/
+
+#define L1OIP_VERSION	0	/* 0...3 */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/mISDNif.h>
+#include <linux/mISDNhw.h>
+#include <linux/mISDNdsp.h>
+#include <linux/init.h>
+#include <linux/in.h>
+#include <linux/inet.h>
+#include <linux/workqueue.h>
+#include <linux/kthread.h>
+#include <net/sock.h>
+#include "core.h"
+#include "l1oip.h"
+
+static const char *l1oip_revision = "2.00";
+
+static int l1oip_cnt;
+static spinlock_t l1oip_lock;
+static struct list_head l1oip_ilist;
+
+#define MAX_CARDS	16
+static u_int type[MAX_CARDS];
+static u_int codec[MAX_CARDS];
+static u_int ip[MAX_CARDS*4];
+static u_int port[MAX_CARDS];
+static u_int remoteport[MAX_CARDS];
+static u_int ondemand[MAX_CARDS];
+static u_int limit[MAX_CARDS];
+static u_int id[MAX_CARDS];
+static int debug;
+static int ulaw;
+
+MODULE_AUTHOR("Andreas Eversberg");
+MODULE_LICENSE("GPL");
+module_param_array(type, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(codec, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(ip, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(port, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(remoteport, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(ondemand, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(limit, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(id, uint, NULL, S_IRUGO | S_IWUSR);
+module_param(ulaw, uint, S_IRUGO | S_IWUSR);
+module_param(debug, uint, S_IRUGO | S_IWUSR);
+
+/*
+ * send a frame via socket, if open and restart timer
+ */
+static int
+l1oip_socket_send(struct l1oip *hc, u8 localcodec, u8 channel, u32 chanmask,
+	u16 timebase, u8 *buf, int len)
+{
+	u8 *p;
+	int multi = 0;
+	u8 frame[len+32];
+	struct socket *socket = NULL;
+	mm_segment_t oldfs;
+
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: sending data to socket (len = %d)\n",
+			__func__, len);
+
+	p = frame;
+
+	/* restart timer */
+	if ((int)(hc->keep_tl.expires-jiffies) < 5*HZ) {
+		del_timer(&hc->keep_tl);
+		hc->keep_tl.expires = jiffies + L1OIP_KEEPALIVE*HZ;
+		add_timer(&hc->keep_tl);
+	} else
+		hc->keep_tl.expires = jiffies + L1OIP_KEEPALIVE*HZ;
+
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: resetting timer\n", __func__);
+
+	/* drop if we have no remote ip or port */
+	if (!hc->sin_remote.sin_addr.s_addr || !hc->sin_remote.sin_port) {
+		if (debug & DEBUG_L1OIP_MSG)
+			printk(KERN_DEBUG "%s: dropping frame, because remote "
+				"IP is not set.\n", __func__);
+		return len;
+	}
+
+	/* assemble frame */
+	*p++ = (L1OIP_VERSION<<6) /* version and coding */
+	     | (hc->pri?0x20:0x00) /* type */
+	     | (hc->id?0x10:0x00) /* id */
+	     | localcodec;
+	if (hc->id) {
+		*p++ = hc->id>>24; /* id */
+		*p++ = hc->id>>16;
+		*p++ = hc->id>>8;
+		*p++ = hc->id;
+	}
+	*p++ = (multi == 1)?0x80:0x00 + channel; /* m-flag, channel */
+	if (multi == 1)
+		*p++ = len; /* length */
+	*p++ = timebase>>8; /* time base */
+	*p++ = timebase;
+
+	if (buf && len) { /* add data to frame */
+		if (localcodec == 1 && ulaw)
+			l1oip_ulaw_to_alaw(buf, len, p);
+		else if (localcodec == 2 && !ulaw)
+			l1oip_alaw_to_ulaw(buf, len, p);
+		else if (localcodec == 3)
+			len = l1oip_law_to_4bit(buf, len, p,
+				&hc->chan[channel].codecstate);
+		else
+			memcpy(p, buf, len);
+	}
+	len += p - frame;
+
+	/* check for socket in safe condition */
+	spin_lock(&hc->socket_lock);
+	if (!hc->socket) {
+		spin_unlock(&hc->socket_lock);
+		return 0;
+	}
+	/* seize socket */
+	socket = hc->socket;
+	hc->socket = NULL;
+	spin_unlock(&hc->socket_lock);
+	/* send packet */
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: sending packet to socket (len "
+			"= %d)\n", __func__, len);
+	hc->sendiov.iov_base = frame;
+	hc->sendiov.iov_len  = len;
+	oldfs = get_fs();
+	set_fs(KERNEL_DS);
+	len = sock_sendmsg(socket, &hc->sendmsg, len);
+	set_fs(oldfs);
+	/* give socket back */
+	hc->socket = socket; /* no locking required */
+
+	return len;
+}
+
+
+/*
+ * receive channel data from socket
+ */
+static void
+l1oip_socket_recv(struct l1oip *hc, u8 remotecodec, u8 channel, u16 timebase,
+	u8 *buf, int len)
+{
+	struct sk_buff *nskb;
+	struct bchannel *bch;
+	struct dchannel *dch;
+	u8 *p;
+	u32 rx_counter;
+
+	if (len == 0) {
+		if (debug & DEBUG_L1OIP_MSG)
+			printk(KERN_DEBUG "%s: received empty keepalive data, "
+				"ignoring\n", __func__);
+		return;
+	}
+
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: received data, sending to mISDN (%d)\n",
+			__func__, len);
+
+	if (channel < 1 || channel > 127) {
+		printk(KERN_WARNING "%s: packet error - channel %d out of "
+			"range\n", __func__, channel);
+		return;
+	}
+	dch = hc->chan[channel].dch;
+	bch = hc->chan[channel].bch;
+	if (!dch && !bch) {
+		printk(KERN_WARNING "%s: packet error - channel %d not in "
+			"stack\n", __func__, channel);
+		return;
+	}
+
+	/* prepare message */
+	nskb = mI_alloc_skb((remotecodec == 3)?(len<<1):len, GFP_ATOMIC);
+	if (!nskb) {
+		printk(KERN_ERR "%s: No mem for skb.\n", __func__);
+		return;
+	}
+	p = skb_put(nskb, (remotecodec == 3)?(len<<1):len);
+
+	if (remotecodec == 1 && ulaw)
+		l1oip_alaw_to_ulaw(buf, len, p);
+	else if (remotecodec == 2 && !ulaw)
+		l1oip_ulaw_to_alaw(buf, len, p);
+	else if (remotecodec == 3)
+		len = l1oip_4bit_to_law(buf, len, p);
+	else
+		memcpy(p, buf, len);
+
+	/* send message up */
+	if (dch && len >= 2) {
+		dch->rx_skb = nskb;
+		recv_Dchannel(dch);
+	}
+	if (bch) {
+		/* expand 16 bit sequence number to 32 bit sequence number */
+		rx_counter = hc->chan[channel].rx_counter;
+		if (((s16)(timebase - rx_counter)) >= 0) {
+			/* time has changed forward */
+			if (timebase >= (rx_counter & 0xffff))
+				rx_counter =
+					(rx_counter & 0xffff0000) | timebase;
+			else
+				rx_counter = ((rx_counter & 0xffff0000)+0x10000)
+					| timebase;
+		} else {
+			/* time has changed backwards */
+			if (timebase < (rx_counter & 0xffff))
+				rx_counter =
+					(rx_counter & 0xffff0000) | timebase;
+			else
+				rx_counter = ((rx_counter & 0xffff0000)-0x10000)
+					| timebase;
+		}
+		hc->chan[channel].rx_counter = rx_counter;
+
+#ifdef REORDER_DEBUG
+		if (hc->chan[channel].disorder_flag) {
+			struct sk_buff *skb;
+			int cnt;
+			skb = hc->chan[channel].disorder_skb;
+			hc->chan[channel].disorder_skb = nskb;
+			nskb = skb;
+			cnt = hc->chan[channel].disorder_cnt;
+			hc->chan[channel].disorder_cnt = rx_counter;
+			rx_counter = cnt;
+		}
+		hc->chan[channel].disorder_flag ^= 1;
+		if (nskb)
+#endif
+		queue_ch_frame(&bch->ch, PH_DATA_IND, rx_counter, nskb);
+	}
+}
+
+
+/*
+ * parse frame and extract channel data
+ */
+static void
+l1oip_socket_parse(struct l1oip *hc, struct sockaddr_in *sin, u8 *buf, int len)
+{
+	u32			id;
+	u8			channel;
+	u8			remotecodec;
+	u16			timebase;
+	int			m, mlen;
+	int			len_start = len; /* initial frame length */
+	struct dchannel		*dch = hc->chan[hc->d_idx].dch;
+
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: received frame, parsing... (%d)\n",
+			__func__, len);
+
+	/* check lenght */
+	if (len < 1+1+2) {
+		printk(KERN_WARNING "%s: packet error - length %d below "
+			"4 bytes\n", __func__, len);
+		return;
+	}
+
+	/* check version */
+	if (((*buf)>>6) != L1OIP_VERSION) {
+		printk(KERN_WARNING "%s: packet error - unknown version %d\n",
+			__func__, buf[0]>>6);
+		return;
+	}
+
+	/* check type */
+	if (((*buf)&0x20) && !hc->pri) {
+		printk(KERN_WARNING "%s: packet error - received E1 packet "
+			"on S0 interface\n", __func__);
+		return;
+	}
+	if (!((*buf)&0x20) && hc->pri) {
+		printk(KERN_WARNING "%s: packet error - received S0 packet "
+			"on E1 interface\n", __func__);
+		return;
+	}
+
+	/* get id flag */
+	id = (*buf>>4)&1;
+
+	/* check coding */
+	remotecodec = (*buf) & 0x0f;
+	if (remotecodec > 3) {
+		printk(KERN_WARNING "%s: packet error - remotecodec %d "
+			"unsupported\n", __func__, remotecodec);
+		return;
+	}
+	buf++;
+	len--;
+
+	/* check id */
+	if (id) {
+		if (!hc->id) {
+			printk(KERN_WARNING "%s: packet error - packet has id "
+				"0x%x, but we have not\n", __func__, id);
+			return;
+		}
+		if (len < 4) {
+			printk(KERN_WARNING "%s: packet error - packet too "
+				"short for ID value\n", __func__);
+			return;
+		}
+		id = (*buf++) << 24;
+		id += (*buf++) << 16;
+		id += (*buf++) << 8;
+		id += (*buf++);
+		len -= 4;
+
+		if (id != hc->id) {
+			printk(KERN_WARNING "%s: packet error - ID mismatch, "
+				"got 0x%x, we 0x%x\n",
+				__func__, id, hc->id);
+			return;
+		}
+	} else {
+		if (hc->id) {
+			printk(KERN_WARNING "%s: packet error - packet has no "
+				"ID, but we have\n", __func__);
+			return;
+		}
+	}
+
+multiframe:
+	if (len < 1) {
+		printk(KERN_WARNING "%s: packet error - packet too short, "
+			"channel expected at position %d.\n",
+			__func__, len-len_start+1);
+		return;
+	}
+
+	/* get channel and multiframe flag */
+	channel = *buf&0x7f;
+	m = *buf >> 7;
+	buf++;
+	len--;
+
+	/* check length on multiframe */
+	if (m) {
+		if (len < 1) {
+			printk(KERN_WARNING "%s: packet error - packet too "
+				"short, length expected at position %d.\n",
+				__func__, len_start-len-1);
+			return;
+		}
+
+		mlen = *buf++;
+		len--;
+		if (mlen == 0)
+			mlen = 256;
+		if (len < mlen+3) {
+			printk(KERN_WARNING "%s: packet error - length %d at "
+				"position %d exceeds total length %d.\n",
+				__func__, mlen, len_start-len-1, len_start);
+			return;
+		}
+		if (len == mlen+3) {
+			printk(KERN_WARNING "%s: packet error - length %d at "
+				"position %d will not allow additional "
+				"packet.\n",
+				__func__, mlen, len_start-len+1);
+			return;
+		}
+	} else
+		mlen = len-2; /* single frame, substract timebase */
+
+	if (len < 2) {
+		printk(KERN_WARNING "%s: packet error - packet too short, time "
+			"base expected at position %d.\n",
+			__func__, len-len_start+1);
+		return;
+	}
+
+	/* get time base */
+	timebase = (*buf++) << 8;
+	timebase |= (*buf++);
+	len -= 2;
+
+	/* if inactive, we send up a PH_ACTIVATE and activate */
+	if (!test_bit(FLG_ACTIVE, &dch->Flags)) {
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: interface become active due to "
+				"received packet\n", __func__);
+		test_and_set_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY, 0,
+			NULL, GFP_ATOMIC);
+	}
+
+	/* distribute packet */
+	l1oip_socket_recv(hc, remotecodec, channel, timebase, buf, mlen);
+	buf += mlen;
+	len -= mlen;
+
+	/* multiframe */
+	if (m)
+		goto multiframe;
+
+	/* restart timer */
+	if ((int)(hc->timeout_tl.expires-jiffies) < 5*HZ || !hc->timeout_on) {
+		hc->timeout_on = 1;
+		del_timer(&hc->timeout_tl);
+		hc->timeout_tl.expires = jiffies + L1OIP_TIMEOUT*HZ;
+		add_timer(&hc->timeout_tl);
+	} else /* only adjust timer */
+		hc->timeout_tl.expires = jiffies + L1OIP_TIMEOUT*HZ;
+
+	/* if ip or source port changes */
+	if ((hc->sin_remote.sin_addr.s_addr != sin->sin_addr.s_addr)
+	 || (hc->sin_remote.sin_port != sin->sin_port)) {
+		if (debug & DEBUG_L1OIP_SOCKET)
+			printk(KERN_DEBUG "%s: remote address changes from "
+				"0x%08x to 0x%08x (port %d to %d)\n", __func__,
+				ntohl(hc->sin_remote.sin_addr.s_addr),
+				ntohl(sin->sin_addr.s_addr),
+				ntohs(hc->sin_remote.sin_port),
+				ntohs(sin->sin_port));
+		hc->sin_remote.sin_addr.s_addr = sin->sin_addr.s_addr;
+		hc->sin_remote.sin_port = sin->sin_port;
+	}
+}
+
+
+/*
+ * socket stuff
+ */
+static int
+l1oip_socket_thread(void *data)
+{
+	struct l1oip *hc = (struct l1oip *)data;
+	int ret = 0;
+	struct msghdr msg;
+	struct iovec iov;
+	mm_segment_t oldfs;
+	struct sockaddr_in sin_rx;
+	unsigned char recvbuf[1500];
+	int recvlen;
+	struct socket *socket = NULL;
+	DECLARE_COMPLETION(wait);
+
+	/* make daemon */
+	allow_signal(SIGTERM);
+
+	/* create socket */
+	if (sock_create(PF_INET, SOCK_DGRAM, IPPROTO_UDP, &socket)) {
+		printk(KERN_ERR "%s: Failed to create socket.\n", __func__);
+		return -EIO;
+	}
+
+	/* set incoming address */
+	hc->sin_local.sin_family = AF_INET;
+	hc->sin_local.sin_addr.s_addr = INADDR_ANY;
+	hc->sin_local.sin_port = htons((unsigned short)hc->localport);
+
+	/* set outgoing address */
+	hc->sin_remote.sin_family = AF_INET;
+	hc->sin_remote.sin_addr.s_addr = htonl(hc->remoteip);
+	hc->sin_remote.sin_port = htons((unsigned short)hc->remoteport);
+
+	/* bind to incomming port */
+	if (socket->ops->bind(socket, (struct sockaddr *)&hc->sin_local,
+	    sizeof(hc->sin_local))) {
+		printk(KERN_ERR "%s: Failed to bind socket to port %d.\n",
+			__func__, hc->localport);
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	/* check sk */
+	if (socket->sk == NULL) {
+		printk(KERN_ERR "%s: socket->sk == NULL\n", __func__);
+		ret = -EIO;
+		goto fail;
+	}
+
+	/* build receive message */
+	msg.msg_name = &sin_rx;
+	msg.msg_namelen = sizeof(sin_rx);
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+
+	/* build send message */
+	hc->sendmsg.msg_name = &hc->sin_remote;
+	hc->sendmsg.msg_namelen = sizeof(hc->sin_remote);
+	hc->sendmsg.msg_control = NULL;
+	hc->sendmsg.msg_controllen = 0;
+	hc->sendmsg.msg_iov    = &hc->sendiov;
+	hc->sendmsg.msg_iovlen = 1;
+
+	/* give away socket */
+	spin_lock(&hc->socket_lock);
+	hc->socket = socket;
+	spin_unlock(&hc->socket_lock);
+
+	/* read loop */
+	if (debug & DEBUG_L1OIP_SOCKET)
+		printk(KERN_DEBUG "%s: socket created and open\n",
+			__func__);
+	while (!signal_pending(current)) {
+		iov.iov_base = recvbuf;
+		iov.iov_len = sizeof(recvbuf);
+		oldfs = get_fs();
+		set_fs(KERNEL_DS);
+		recvlen = sock_recvmsg(socket, &msg, sizeof(recvbuf), 0);
+		set_fs(oldfs);
+		if (recvlen > 0) {
+			l1oip_socket_parse(hc, &sin_rx, recvbuf, recvlen);
+		} else {
+			if (debug & DEBUG_L1OIP_SOCKET)
+			    printk(KERN_WARNING "%s: broken pipe on socket\n",
+				__func__);
+		}
+	}
+
+	/* get socket back, check first if in use, maybe by send function */
+	spin_lock(&hc->socket_lock);
+	/* if hc->socket is NULL, it is in use until it is given back */
+	while (!hc->socket) {
+		spin_unlock(&hc->socket_lock);
+		schedule_timeout(HZ/10);
+		spin_lock(&hc->socket_lock);
+	}
+	hc->socket = NULL;
+	spin_unlock(&hc->socket_lock);
+
+	if (debug & DEBUG_L1OIP_SOCKET)
+		printk(KERN_DEBUG "%s: socket thread terminating\n",
+			__func__);
+
+fail:
+	/* close socket */
+	if (socket)
+		sock_release(socket);
+
+	/* if we got killed, signal completion */
+	complete(&hc->socket_complete);
+	hc->socket_thread = NULL; /* show termination of thread */
+
+	if (debug & DEBUG_L1OIP_SOCKET)
+		printk(KERN_DEBUG "%s: socket thread terminated\n",
+			__func__);
+	return ret;
+}
+
+static void
+l1oip_socket_close(struct l1oip *hc)
+{
+	/* kill thread */
+	if (hc->socket_thread) {
+		if (debug & DEBUG_L1OIP_SOCKET)
+			printk(KERN_DEBUG "%s: socket thread exists, "
+				"killing...\n", __func__);
+		send_sig(SIGTERM, hc->socket_thread, 0);
+		wait_for_completion(&hc->socket_complete);
+	}
+}
+
+static int
+l1oip_socket_open(struct l1oip *hc)
+{
+	/* in case of reopen, we need to close first */
+	l1oip_socket_close(hc);
+
+	init_completion(&hc->socket_complete);
+
+	/* create receive process */
+	hc->socket_thread = kthread_run(l1oip_socket_thread, hc, "l1oip_%s",
+		hc->name);
+	if (IS_ERR(hc->socket_thread)) {
+		int err = PTR_ERR(hc->socket_thread);
+		printk(KERN_ERR "%s: Failed (%d) to create socket process.\n",
+			__func__, err);
+		hc->socket_thread = NULL;
+		sock_release(hc->socket);
+		return err;
+	}
+	if (debug & DEBUG_L1OIP_SOCKET)
+		printk(KERN_DEBUG "%s: socket thread created\n", __func__);
+
+	return 0;
+}
+
+
+static void
+l1oip_send_bh(struct work_struct *work)
+{
+	struct l1oip *hc = container_of(work, struct l1oip, workq);
+
+	if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+		printk(KERN_DEBUG "%s: keepalive timer expired, sending empty "
+			"frame on dchannel\n", __func__);
+
+	/* send an empty l1oip frame at D-channel */
+	l1oip_socket_send(hc, 0, hc->d_idx, 0, 0, NULL, 0);
+}
+
+
+/*
+ * timer stuff
+ */
+static void
+l1oip_keepalive(void *data)
+{
+	struct l1oip *hc = (struct l1oip *)data;
+
+	schedule_work(&hc->workq);
+}
+
+static void
+l1oip_timeout(void *data)
+{
+	struct l1oip			*hc = (struct l1oip *)data;
+	struct dchannel		*dch = hc->chan[hc->d_idx].dch;
+
+	if (debug & DEBUG_L1OIP_MSG)
+		printk(KERN_DEBUG "%s: timeout timer expired, turn layer one "
+			"down.\n", __func__);
+
+	hc->timeout_on = 0; /* state that timer must be initialized next time */
+
+	/* if timeout, we send up a PH_DEACTIVATE and deactivate */
+	if (test_bit(FLG_ACTIVE, &dch->Flags)) {
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: interface become deactivated "
+				"due to timeout\n", __func__);
+		test_and_clear_bit(FLG_ACTIVE, &dch->Flags);
+		_queue_data(&dch->dev.D, PH_DEACTIVATE_IND, MISDN_ID_ANY, 0,
+			NULL, GFP_ATOMIC);
+	}
+
+	/* if we have ondemand set, we remove ip address */
+	if (hc->ondemand) {
+		if (debug & DEBUG_L1OIP_MSG)
+			printk(KERN_DEBUG "%s: on demand causes ip address to "
+				"be removed\n", __func__);
+		hc->sin_remote.sin_addr.s_addr = 0;
+	}
+}
+
+
+/*
+ * message handling
+ */
+static int
+handle_dmsg(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct l1oip			*hc = dch->hw;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	int			ret = -EINVAL;
+	int			l, ll;
+	unsigned char		*p;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		if (skb->len < 1) {
+			printk(KERN_WARNING "%s: skb too small\n",
+				__func__);
+			break;
+		}
+		if (skb->len > MAX_DFRAME_LEN_L1 || skb->len > L1OIP_MAX_LEN) {
+			printk(KERN_WARNING "%s: skb too large\n",
+				__func__);
+			break;
+		}
+		/* send frame */
+		p = skb->data;
+		l = skb->len;
+		while (l) {
+			ll = (l < L1OIP_MAX_PERFRAME)?l:L1OIP_MAX_PERFRAME;
+			l1oip_socket_send(hc, 0, dch->slot, 0,
+				hc->chan[dch->slot].tx_counter++, p, ll);
+			p += ll;
+			l -= ll;
+		}
+		skb_trim(skb, 0);
+		queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb);
+		return 0;
+	case PH_ACTIVATE_REQ:
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: PH_ACTIVATE channel %d (1..%d)\n"
+				, __func__, dch->slot, hc->b_num+1);
+		skb_trim(skb, 0);
+		if (test_bit(FLG_ACTIVE, &dch->Flags))
+			queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb);
+		else
+			queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb);
+		return 0;
+	case PH_DEACTIVATE_REQ:
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: PH_DEACTIVATE channel %d "
+				"(1..%d)\n", __func__, dch->slot,
+				hc->b_num+1);
+		skb_trim(skb, 0);
+		if (test_bit(FLG_ACTIVE, &dch->Flags))
+			queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb);
+		else
+			queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb);
+		return 0;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int
+channel_dctrl(struct dchannel *dch, struct mISDN_ctrl_req *cq)
+{
+	int	ret = 0;
+	struct l1oip	*hc = dch->hw;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = MISDN_CTRL_SETPEER | MISDN_CTRL_UNSETPEER;
+		break;
+	case MISDN_CTRL_SETPEER:
+		hc->remoteip = (u32)cq->p1;
+		hc->remoteport = cq->p2 & 0xffff;
+		hc->localport = cq->p2 >> 16;
+		if (!hc->remoteport)
+			hc->remoteport = hc->localport;
+		if (debug & DEBUG_L1OIP_SOCKET)
+			printk(KERN_DEBUG "%s: got new ip address from user "
+				"space.\n", __func__);
+			l1oip_socket_open(hc);
+		break;
+	case MISDN_CTRL_UNSETPEER:
+		if (debug & DEBUG_L1OIP_SOCKET)
+			printk(KERN_DEBUG "%s: removing ip address.\n",
+				__func__);
+		hc->remoteip = 0;
+		l1oip_socket_open(hc);
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n",
+		    __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int
+open_dchannel(struct l1oip *hc, struct dchannel *dch, struct channel_req *rq)
+{
+	if (debug & DEBUG_HW_OPEN)
+		printk(KERN_DEBUG "%s: dev(%d) open from %p\n", __func__,
+		    dch->dev.id, __builtin_return_address(0));
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	if ((dch->dev.D.protocol != ISDN_P_NONE) &&
+	    (dch->dev.D.protocol != rq->protocol)) {
+		if (debug & DEBUG_HW_OPEN)
+			printk(KERN_WARNING "%s: change protocol %x to %x\n",
+			__func__, dch->dev.D.protocol, rq->protocol);
+	}
+	if (dch->dev.D.protocol != rq->protocol)
+		dch->dev.D.protocol = rq->protocol;
+
+	if (test_bit(FLG_ACTIVE, &dch->Flags)) {
+		_queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY,
+		    0, NULL, GFP_KERNEL);
+	}
+	rq->ch = &dch->dev.D;
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+static int
+open_bchannel(struct l1oip *hc, struct dchannel *dch, struct channel_req *rq)
+{
+	struct bchannel	*bch;
+	int		ch;
+
+	if (!test_bit(rq->adr.channel & 0x1f,
+		&dch->dev.channelmap[rq->adr.channel >> 5]))
+		return -EINVAL;
+	if (rq->protocol == ISDN_P_NONE)
+		return -EINVAL;
+	ch = rq->adr.channel; /* BRI: 1=B1 2=B2  PRI: 1..15,17.. */
+	bch = hc->chan[ch].bch;
+	if (!bch) {
+		printk(KERN_ERR "%s:internal error ch %d has no bch\n",
+		    __func__, ch);
+		return -EINVAL;
+	}
+	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
+		return -EBUSY; /* b-channel can be only open once */
+	bch->ch.protocol = rq->protocol;
+	rq->ch = &bch->ch;
+	if (!try_module_get(THIS_MODULE))
+		printk(KERN_WARNING "%s:cannot get module\n", __func__);
+	return 0;
+}
+
+static int
+l1oip_dctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct mISDNdevice	*dev = container_of(ch, struct mISDNdevice, D);
+	struct dchannel		*dch = container_of(dev, struct dchannel, dev);
+	struct l1oip			*hc = dch->hw;
+	struct channel_req	*rq;
+	int			err = 0;
+
+	if (dch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n",
+		    __func__, cmd, arg);
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		rq = arg;
+		switch (rq->protocol) {
+		case ISDN_P_TE_S0:
+		case ISDN_P_NT_S0:
+			if (hc->pri) {
+				err = -EINVAL;
+				break;
+			}
+			err = open_dchannel(hc, dch, rq);
+			break;
+		case ISDN_P_TE_E1:
+		case ISDN_P_NT_E1:
+			if (!hc->pri) {
+				err = -EINVAL;
+				break;
+			}
+			err = open_dchannel(hc, dch, rq);
+			break;
+		default:
+			err = open_bchannel(hc, dch, rq);
+		}
+		break;
+	case CLOSE_CHANNEL:
+		if (debug & DEBUG_HW_OPEN)
+			printk(KERN_DEBUG "%s: dev(%d) close from %p\n",
+			    __func__, dch->dev.id,
+			    __builtin_return_address(0));
+		module_put(THIS_MODULE);
+		break;
+	case CONTROL_CHANNEL:
+		err = channel_dctrl(dch, arg);
+		break;
+	default:
+		if (dch->debug & DEBUG_HW)
+			printk(KERN_DEBUG "%s: unknown command %x\n",
+			    __func__, cmd);
+		err = -EINVAL;
+	}
+	return err;
+}
+
+static int
+handle_bmsg(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct bchannel		*bch = container_of(ch, struct bchannel, ch);
+	struct l1oip			*hc = bch->hw;
+	int			ret = -EINVAL;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	int			l, ll, i;
+	unsigned char		*p;
+
+	switch (hh->prim) {
+	case PH_DATA_REQ:
+		if (skb->len <= 0) {
+			printk(KERN_WARNING "%s: skb too small\n",
+				__func__);
+			break;
+		}
+		if (skb->len > MAX_DFRAME_LEN_L1 || skb->len > L1OIP_MAX_LEN) {
+			printk(KERN_WARNING "%s: skb too large\n",
+				__func__);
+			break;
+		}
+		/* check for AIS / ulaw-silence */
+		p = skb->data;
+		l = skb->len;
+		for (i = 0; i < l; i++) {
+			if (*p++ != 0xff)
+				break;
+		}
+		if (i == l) {
+			if (debug & DEBUG_L1OIP_MSG)
+				printk(KERN_DEBUG "%s: got AIS, not sending, "
+					"but counting\n", __func__);
+			hc->chan[bch->slot].tx_counter += l;
+			skb_trim(skb, 0);
+			queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb);
+			return 0;
+		}
+		/* check for silence */
+		p = skb->data;
+		l = skb->len;
+		for (i = 0; i < l; i++) {
+			if (*p++ != 0x2a)
+				break;
+		}
+		if (i == l) {
+			if (debug & DEBUG_L1OIP_MSG)
+				printk(KERN_DEBUG "%s: got silence, not sending"
+					", but counting\n", __func__);
+			hc->chan[bch->slot].tx_counter += l;
+			skb_trim(skb, 0);
+			queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb);
+			return 0;
+		}
+
+		/* send frame */
+		p = skb->data;
+		l = skb->len;
+		while (l) {
+			ll = (l < L1OIP_MAX_PERFRAME)?l:L1OIP_MAX_PERFRAME;
+			l1oip_socket_send(hc, hc->codec, bch->slot, 0,
+				hc->chan[bch->slot].tx_counter, p, ll);
+			hc->chan[bch->slot].tx_counter += ll;
+			p += ll;
+			l -= ll;
+		}
+		skb_trim(skb, 0);
+		queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb);
+		return 0;
+	case PH_ACTIVATE_REQ:
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: PH_ACTIVATE channel %d (1..%d)\n"
+				, __func__, bch->slot, hc->b_num+1);
+		hc->chan[bch->slot].codecstate = 0;
+		test_and_set_bit(FLG_ACTIVE, &bch->Flags);
+		skb_trim(skb, 0);
+		queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb);
+		return 0;
+	case PH_DEACTIVATE_REQ:
+		if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET))
+			printk(KERN_DEBUG "%s: PH_DEACTIVATE channel %d "
+				"(1..%d)\n", __func__, bch->slot,
+				hc->b_num+1);
+		test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
+		skb_trim(skb, 0);
+		queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb);
+		return 0;
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int
+channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
+{
+	int			ret = 0;
+	struct dsp_features	*features =
+		(struct dsp_features *)(*((u_long *)&cq->p1));
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = MISDN_CTRL_HW_FEATURES_OP;
+		break;
+	case MISDN_CTRL_HW_FEATURES: /* fill features structure */
+		if (debug & DEBUG_L1OIP_MSG)
+			printk(KERN_DEBUG "%s: HW_FEATURE request\n",
+			    __func__);
+		/* create confirm */
+		features->unclocked = 1;
+		features->unordered = 1;
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown Op %x\n",
+		    __func__, cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int
+l1oip_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct bchannel	*bch = container_of(ch, struct bchannel, ch);
+	int		err = -EINVAL;
+
+	if (bch->debug & DEBUG_HW)
+		printk(KERN_DEBUG "%s: cmd:%x %p\n",
+		    __func__, cmd, arg);
+	switch (cmd) {
+	case CLOSE_CHANNEL:
+		test_and_clear_bit(FLG_OPEN, &bch->Flags);
+		test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
+		ch->protocol = ISDN_P_NONE;
+		ch->peer = NULL;
+		module_put(THIS_MODULE);
+		err = 0;
+		break;
+	case CONTROL_CHANNEL:
+		err = channel_bctrl(bch, arg);
+		break;
+	default:
+		printk(KERN_WARNING "%s: unknown prim(%x)\n",
+			__func__, cmd);
+	}
+	return err;
+}
+
+
+/*
+ * cleanup module and stack
+ */
+static void
+release_card(struct l1oip *hc)
+{
+	int	ch;
+
+	if (timer_pending(&hc->keep_tl))
+		del_timer(&hc->keep_tl);
+
+	if (timer_pending(&hc->timeout_tl))
+		del_timer(&hc->timeout_tl);
+
+	if (hc->socket_thread)
+		l1oip_socket_close(hc);
+
+	if (hc->registered && hc->chan[hc->d_idx].dch)
+		mISDN_unregister_device(&hc->chan[hc->d_idx].dch->dev);
+	for (ch = 0; ch < 128; ch++) {
+		if (hc->chan[ch].dch) {
+			mISDN_freedchannel(hc->chan[ch].dch);
+			kfree(hc->chan[ch].dch);
+		}
+		if (hc->chan[ch].bch) {
+			mISDN_freebchannel(hc->chan[ch].bch);
+			kfree(hc->chan[ch].bch);
+#ifdef REORDER_DEBUG
+			if (hc->chan[ch].disorder_skb)
+				dev_kfree_skb(hc->chan[ch].disorder_skb);
+#endif
+		}
+	}
+
+	spin_lock(&l1oip_lock);
+	list_del(&hc->list);
+	spin_unlock(&l1oip_lock);
+
+	kfree(hc);
+}
+
+static void
+l1oip_cleanup(void)
+{
+	struct l1oip *hc, *next;
+
+	list_for_each_entry_safe(hc, next, &l1oip_ilist, list)
+		release_card(hc);
+
+	l1oip_4bit_free();
+}
+
+
+/*
+ * module and stack init
+ */
+static int
+init_card(struct l1oip *hc, int pri, int bundle)
+{
+	struct dchannel	*dch;
+	struct bchannel	*bch;
+	int		ret;
+	int		i, ch;
+
+	spin_lock_init(&hc->socket_lock);
+	hc->idx = l1oip_cnt;
+	hc->pri = pri;
+	hc->d_idx = pri?16:3;
+	hc->b_num = pri?30:2;
+	hc->bundle = bundle;
+	if (hc->pri)
+		sprintf(hc->name, "l1oip-e1.%d", l1oip_cnt + 1);
+	else
+		sprintf(hc->name, "l1oip-s0.%d", l1oip_cnt + 1);
+
+	switch (codec[l1oip_cnt]) {
+	case 0: /* as is */
+	case 1: /* alaw */
+	case 2: /* ulaw */
+	case 3: /* 4bit */
+		break;
+	default:
+		printk(KERN_ERR "Codec(%d) not supported.\n",
+			codec[l1oip_cnt]);
+		return -EINVAL;
+	}
+	hc->codec = codec[l1oip_cnt];
+	if (debug & DEBUG_L1OIP_INIT)
+		printk(KERN_DEBUG "%s: using codec %d\n",
+			__func__, hc->codec);
+
+	if (id[l1oip_cnt] == 0) {
+		printk(KERN_WARNING "Warning: No 'id' value given or "
+			"0, this is highly unsecure. Please use 32 "
+			"bit randmom number 0x...\n");
+	}
+	hc->id = id[l1oip_cnt];
+	if (debug & DEBUG_L1OIP_INIT)
+		printk(KERN_DEBUG "%s: using id 0x%x\n", __func__, hc->id);
+
+	hc->ondemand = ondemand[l1oip_cnt];
+	if (hc->ondemand && !hc->id) {
+		printk(KERN_ERR "%s: ondemand option only allowed in "
+			"conjunction with non 0 ID\n", __func__);
+		return -EINVAL;
+	}
+
+	if (limit[l1oip_cnt])
+		hc->b_num = limit[l1oip_cnt];
+	if (!pri && hc->b_num > 2) {
+		printk(KERN_ERR "Maximum limit for BRI interface is 2 "
+			"channels.\n");
+		return -EINVAL;
+	}
+	if (pri && hc->b_num > 126) {
+		printk(KERN_ERR "Maximum limit for PRI interface is 126 "
+			"channels.\n");
+		return -EINVAL;
+	}
+	if (pri && hc->b_num > 30) {
+		printk(KERN_WARNING "Maximum limit for BRI interface is 30 "
+			"channels.\n");
+		printk(KERN_WARNING "Your selection of %d channels must be "
+			"supported by application.\n", hc->limit);
+	}
+
+	hc->remoteip = ip[l1oip_cnt<<2] << 24
+		     | ip[(l1oip_cnt<<2)+1] << 16
+		     | ip[(l1oip_cnt<<2)+2] << 8
+		     | ip[(l1oip_cnt<<2)+3];
+	hc->localport = port[l1oip_cnt]?:(L1OIP_DEFAULTPORT+l1oip_cnt);
+	if (remoteport[l1oip_cnt])
+		hc->remoteport = remoteport[l1oip_cnt];
+	else
+		hc->remoteport = hc->localport;
+	if (debug & DEBUG_L1OIP_INIT)
+		printk(KERN_DEBUG "%s: using local port %d remote ip "
+			"%d.%d.%d.%d port %d ondemand %d\n", __func__,
+			hc->localport, hc->remoteip >> 24,
+			(hc->remoteip >> 16) & 0xff,
+			(hc->remoteip >> 8) & 0xff, hc->remoteip & 0xff,
+			hc->remoteport, hc->ondemand);
+
+	dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL);
+	if (!dch)
+		return -ENOMEM;
+	dch->debug = debug;
+	mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, NULL);
+	dch->hw = hc;
+	if (pri)
+		dch->dev.Dprotocols = (1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1);
+	else
+		dch->dev.Dprotocols = (1 << ISDN_P_TE_S0) | (1 << ISDN_P_NT_S0);
+	dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
+	    (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
+	dch->dev.D.send = handle_dmsg;
+	dch->dev.D.ctrl = l1oip_dctrl;
+	dch->dev.nrbchan = hc->b_num;
+	dch->slot = hc->d_idx;
+	hc->chan[hc->d_idx].dch = dch;
+	i = 1;
+	for (ch = 0; ch < dch->dev.nrbchan; ch++) {
+		if (ch == 15)
+			i++;
+		bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL);
+		if (!bch) {
+			printk(KERN_ERR "%s: no memory for bchannel\n",
+			    __func__);
+			return -ENOMEM;
+		}
+		bch->nr = i + ch;
+		bch->slot = i + ch;
+		bch->debug = debug;
+		mISDN_initbchannel(bch, MAX_DATA_MEM);
+		bch->hw = hc;
+		bch->ch.send = handle_bmsg;
+		bch->ch.ctrl = l1oip_bctrl;
+		bch->ch.nr = i + ch;
+		list_add(&bch->ch.list, &dch->dev.bchannels);
+		hc->chan[i + ch].bch = bch;
+		test_and_set_bit(bch->nr & 0x1f,
+			&dch->dev.channelmap[bch->nr >> 5]);
+	}
+	ret = mISDN_register_device(&dch->dev, hc->name);
+	if (ret)
+		return ret;
+	hc->registered = 1;
+
+	if (debug & DEBUG_L1OIP_INIT)
+		printk(KERN_DEBUG "%s: Setting up network card(%d)\n",
+			__func__, l1oip_cnt + 1);
+	ret = l1oip_socket_open(hc);
+	if (ret)
+		return ret;
+
+	hc->keep_tl.function = (void *)l1oip_keepalive;
+	hc->keep_tl.data = (ulong)hc;
+	init_timer(&hc->keep_tl);
+	hc->keep_tl.expires = jiffies + 2*HZ; /* two seconds first time */
+	add_timer(&hc->keep_tl);
+
+	hc->timeout_tl.function = (void *)l1oip_timeout;
+	hc->timeout_tl.data = (ulong)hc;
+	init_timer(&hc->timeout_tl);
+	hc->timeout_on = 0; /* state that we have timer off */
+
+	return 0;
+}
+
+static int __init
+l1oip_init(void)
+{
+	int		pri, bundle;
+	struct l1oip		*hc;
+	int		ret;
+
+	printk(KERN_INFO "mISDN: Layer-1-over-IP driver Rev. %s\n",
+		l1oip_revision);
+
+	INIT_LIST_HEAD(&l1oip_ilist);
+	spin_lock_init(&l1oip_lock);
+
+	if (l1oip_4bit_alloc(ulaw))
+		return -ENOMEM;
+
+	l1oip_cnt = 0;
+	while (type[l1oip_cnt] && l1oip_cnt < MAX_CARDS) {
+		switch (type[l1oip_cnt] & 0xff) {
+		case 1:
+			pri = 0;
+			bundle = 0;
+			break;
+		case 2:
+			pri = 1;
+			bundle = 0;
+			break;
+		case 3:
+			pri = 0;
+			bundle = 1;
+			break;
+		case 4:
+			pri = 1;
+			bundle = 1;
+			break;
+		default:
+			printk(KERN_ERR "Card type(%d) not supported.\n",
+				type[l1oip_cnt] & 0xff);
+			l1oip_cleanup();
+			return -EINVAL;
+		}
+
+		if (debug & DEBUG_L1OIP_INIT)
+			printk(KERN_DEBUG "%s: interface %d is %s with %s.\n",
+				__func__, l1oip_cnt, pri?"PRI":"BRI",
+				bundle?"bundled IP packet for all B-channels"
+				 :"seperate IP packets for every B-channel");
+
+		hc = kzalloc(sizeof(struct l1oip), GFP_ATOMIC);
+		if (!hc) {
+			printk(KERN_ERR "No kmem for L1-over-IP driver.\n");
+			l1oip_cleanup();
+			return -ENOMEM;
+		}
+		INIT_WORK(&hc->workq, (void *)l1oip_send_bh);
+
+		spin_lock(&l1oip_lock);
+		list_add_tail(&hc->list, &l1oip_ilist);
+		spin_unlock(&l1oip_lock);
+
+		ret = init_card(hc, pri, bundle);
+		if (ret) {
+			l1oip_cleanup();
+			return ret;
+		}
+
+		l1oip_cnt++;
+	}
+	printk(KERN_INFO "%d virtual devices registered\n", l1oip_cnt);
+	return 0;
+}
+
+module_init(l1oip_init);
+module_exit(l1oip_cleanup);
+
diff --git a/drivers/isdn/mISDN/layer1.c b/drivers/isdn/mISDN/layer1.c
new file mode 100644
index 00000000000..fced1a2755f
--- /dev/null
+++ b/drivers/isdn/mISDN/layer1.c
@@ -0,0 +1,403 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+
+#include <linux/module.h>
+#include <linux/mISDNhw.h>
+#include "layer1.h"
+#include "fsm.h"
+
+static int *debug;
+
+struct layer1 {
+	u_long			Flags;
+	struct FsmInst		l1m;
+	struct FsmTimer 	timer;
+	int			delay;
+	struct dchannel		*dch;
+	dchannel_l1callback	*dcb;
+};
+
+#define TIMER3_VALUE 7000
+
+static
+struct Fsm l1fsm_s = {NULL, 0, 0, NULL, NULL};
+
+enum {
+	ST_L1_F2,
+	ST_L1_F3,
+	ST_L1_F4,
+	ST_L1_F5,
+	ST_L1_F6,
+	ST_L1_F7,
+	ST_L1_F8,
+};
+
+#define L1S_STATE_COUNT (ST_L1_F8+1)
+
+static char *strL1SState[] =
+{
+	"ST_L1_F2",
+	"ST_L1_F3",
+	"ST_L1_F4",
+	"ST_L1_F5",
+	"ST_L1_F6",
+	"ST_L1_F7",
+	"ST_L1_F8",
+};
+
+enum {
+	EV_PH_ACTIVATE,
+	EV_PH_DEACTIVATE,
+	EV_RESET_IND,
+	EV_DEACT_CNF,
+	EV_DEACT_IND,
+	EV_POWER_UP,
+	EV_ANYSIG_IND,
+	EV_INFO2_IND,
+	EV_INFO4_IND,
+	EV_TIMER_DEACT,
+	EV_TIMER_ACT,
+	EV_TIMER3,
+};
+
+#define L1_EVENT_COUNT (EV_TIMER3 + 1)
+
+static char *strL1Event[] =
+{
+	"EV_PH_ACTIVATE",
+	"EV_PH_DEACTIVATE",
+	"EV_RESET_IND",
+	"EV_DEACT_CNF",
+	"EV_DEACT_IND",
+	"EV_POWER_UP",
+	"EV_ANYSIG_IND",
+	"EV_INFO2_IND",
+	"EV_INFO4_IND",
+	"EV_TIMER_DEACT",
+	"EV_TIMER_ACT",
+	"EV_TIMER3",
+};
+
+static void
+l1m_debug(struct FsmInst *fi, char *fmt, ...)
+{
+	struct layer1 *l1 = fi->userdata;
+	va_list va;
+
+	va_start(va, fmt);
+	printk(KERN_DEBUG "%s: ", l1->dch->dev.name);
+	vprintk(fmt, va);
+	printk("\n");
+	va_end(va);
+}
+
+static void
+l1_reset(struct FsmInst *fi, int event, void *arg)
+{
+	mISDN_FsmChangeState(fi, ST_L1_F3);
+}
+
+static void
+l1_deact_cnf(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	mISDN_FsmChangeState(fi, ST_L1_F3);
+	if (test_bit(FLG_L1_ACTIVATING, &l1->Flags))
+		l1->dcb(l1->dch, HW_POWERUP_REQ);
+}
+
+static void
+l1_deact_req_s(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	mISDN_FsmChangeState(fi, ST_L1_F3);
+	mISDN_FsmRestartTimer(&l1->timer, 550, EV_TIMER_DEACT, NULL, 2);
+	test_and_set_bit(FLG_L1_DEACTTIMER, &l1->Flags);
+}
+
+static void
+l1_power_up_s(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	if (test_bit(FLG_L1_ACTIVATING, &l1->Flags)) {
+		mISDN_FsmChangeState(fi, ST_L1_F4);
+		l1->dcb(l1->dch, INFO3_P8);
+	} else
+		mISDN_FsmChangeState(fi, ST_L1_F3);
+}
+
+static void
+l1_go_F5(struct FsmInst *fi, int event, void *arg)
+{
+	mISDN_FsmChangeState(fi, ST_L1_F5);
+}
+
+static void
+l1_go_F8(struct FsmInst *fi, int event, void *arg)
+{
+	mISDN_FsmChangeState(fi, ST_L1_F8);
+}
+
+static void
+l1_info2_ind(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	mISDN_FsmChangeState(fi, ST_L1_F6);
+	l1->dcb(l1->dch, INFO3_P8);
+}
+
+static void
+l1_info4_ind(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	mISDN_FsmChangeState(fi, ST_L1_F7);
+	l1->dcb(l1->dch, INFO3_P8);
+	if (test_and_clear_bit(FLG_L1_DEACTTIMER, &l1->Flags))
+		mISDN_FsmDelTimer(&l1->timer, 4);
+	if (!test_bit(FLG_L1_ACTIVATED, &l1->Flags)) {
+		if (test_and_clear_bit(FLG_L1_T3RUN, &l1->Flags))
+			mISDN_FsmDelTimer(&l1->timer, 3);
+		mISDN_FsmRestartTimer(&l1->timer, 110, EV_TIMER_ACT, NULL, 2);
+		test_and_set_bit(FLG_L1_ACTTIMER, &l1->Flags);
+	}
+}
+
+static void
+l1_timer3(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	test_and_clear_bit(FLG_L1_T3RUN, &l1->Flags);
+	if (test_and_clear_bit(FLG_L1_ACTIVATING, &l1->Flags)) {
+		if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags))
+			l1->dcb(l1->dch, HW_D_NOBLOCKED);
+		l1->dcb(l1->dch, PH_DEACTIVATE_IND);
+	}
+	if (l1->l1m.state != ST_L1_F6) {
+		mISDN_FsmChangeState(fi, ST_L1_F3);
+		l1->dcb(l1->dch, HW_POWERUP_REQ);
+	}
+}
+
+static void
+l1_timer_act(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	test_and_clear_bit(FLG_L1_ACTTIMER, &l1->Flags);
+	test_and_set_bit(FLG_L1_ACTIVATED, &l1->Flags);
+	l1->dcb(l1->dch, PH_ACTIVATE_IND);
+}
+
+static void
+l1_timer_deact(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	test_and_clear_bit(FLG_L1_DEACTTIMER, &l1->Flags);
+	test_and_clear_bit(FLG_L1_ACTIVATED, &l1->Flags);
+	if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags))
+		l1->dcb(l1->dch, HW_D_NOBLOCKED);
+	l1->dcb(l1->dch, PH_DEACTIVATE_IND);
+	l1->dcb(l1->dch, HW_DEACT_REQ);
+}
+
+static void
+l1_activate_s(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	mISDN_FsmRestartTimer(&l1->timer, TIMER3_VALUE, EV_TIMER3, NULL, 2);
+	test_and_set_bit(FLG_L1_T3RUN, &l1->Flags);
+	l1->dcb(l1->dch, HW_RESET_REQ);
+}
+
+static void
+l1_activate_no(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer1 *l1 = fi->userdata;
+
+	if ((!test_bit(FLG_L1_DEACTTIMER, &l1->Flags)) &&
+	    (!test_bit(FLG_L1_T3RUN, &l1->Flags))) {
+		test_and_clear_bit(FLG_L1_ACTIVATING, &l1->Flags);
+		if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags))
+			l1->dcb(l1->dch, HW_D_NOBLOCKED);
+		l1->dcb(l1->dch, PH_DEACTIVATE_IND);
+	}
+}
+
+static struct FsmNode L1SFnList[] =
+{
+	{ST_L1_F3, EV_PH_ACTIVATE, l1_activate_s},
+	{ST_L1_F6, EV_PH_ACTIVATE, l1_activate_no},
+	{ST_L1_F8, EV_PH_ACTIVATE, l1_activate_no},
+	{ST_L1_F3, EV_RESET_IND, l1_reset},
+	{ST_L1_F4, EV_RESET_IND, l1_reset},
+	{ST_L1_F5, EV_RESET_IND, l1_reset},
+	{ST_L1_F6, EV_RESET_IND, l1_reset},
+	{ST_L1_F7, EV_RESET_IND, l1_reset},
+	{ST_L1_F8, EV_RESET_IND, l1_reset},
+	{ST_L1_F3, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F4, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F5, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F6, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F7, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F8, EV_DEACT_CNF, l1_deact_cnf},
+	{ST_L1_F6, EV_DEACT_IND, l1_deact_req_s},
+	{ST_L1_F7, EV_DEACT_IND, l1_deact_req_s},
+	{ST_L1_F8, EV_DEACT_IND, l1_deact_req_s},
+	{ST_L1_F3, EV_POWER_UP,  l1_power_up_s},
+	{ST_L1_F4, EV_ANYSIG_IND, l1_go_F5},
+	{ST_L1_F6, EV_ANYSIG_IND, l1_go_F8},
+	{ST_L1_F7, EV_ANYSIG_IND, l1_go_F8},
+	{ST_L1_F3, EV_INFO2_IND, l1_info2_ind},
+	{ST_L1_F4, EV_INFO2_IND, l1_info2_ind},
+	{ST_L1_F5, EV_INFO2_IND, l1_info2_ind},
+	{ST_L1_F7, EV_INFO2_IND, l1_info2_ind},
+	{ST_L1_F8, EV_INFO2_IND, l1_info2_ind},
+	{ST_L1_F3, EV_INFO4_IND, l1_info4_ind},
+	{ST_L1_F4, EV_INFO4_IND, l1_info4_ind},
+	{ST_L1_F5, EV_INFO4_IND, l1_info4_ind},
+	{ST_L1_F6, EV_INFO4_IND, l1_info4_ind},
+	{ST_L1_F8, EV_INFO4_IND, l1_info4_ind},
+	{ST_L1_F3, EV_TIMER3, l1_timer3},
+	{ST_L1_F4, EV_TIMER3, l1_timer3},
+	{ST_L1_F5, EV_TIMER3, l1_timer3},
+	{ST_L1_F6, EV_TIMER3, l1_timer3},
+	{ST_L1_F8, EV_TIMER3, l1_timer3},
+	{ST_L1_F7, EV_TIMER_ACT, l1_timer_act},
+	{ST_L1_F3, EV_TIMER_DEACT, l1_timer_deact},
+	{ST_L1_F4, EV_TIMER_DEACT, l1_timer_deact},
+	{ST_L1_F5, EV_TIMER_DEACT, l1_timer_deact},
+	{ST_L1_F6, EV_TIMER_DEACT, l1_timer_deact},
+	{ST_L1_F7, EV_TIMER_DEACT, l1_timer_deact},
+	{ST_L1_F8, EV_TIMER_DEACT, l1_timer_deact},
+};
+
+static void
+release_l1(struct layer1 *l1) {
+	mISDN_FsmDelTimer(&l1->timer, 0);
+	if (l1->dch)
+		l1->dch->l1 = NULL;
+	module_put(THIS_MODULE);
+	kfree(l1);
+}
+
+int
+l1_event(struct layer1 *l1, u_int event)
+{
+	int		err = 0;
+
+	if (!l1)
+		return -EINVAL;
+	switch (event) {
+	case HW_RESET_IND:
+		mISDN_FsmEvent(&l1->l1m, EV_RESET_IND, NULL);
+		break;
+	case HW_DEACT_IND:
+		mISDN_FsmEvent(&l1->l1m, EV_DEACT_IND, NULL);
+		break;
+	case HW_POWERUP_IND:
+		mISDN_FsmEvent(&l1->l1m, EV_POWER_UP, NULL);
+		break;
+	case HW_DEACT_CNF:
+		mISDN_FsmEvent(&l1->l1m, EV_DEACT_CNF, NULL);
+		break;
+	case ANYSIGNAL:
+		mISDN_FsmEvent(&l1->l1m, EV_ANYSIG_IND, NULL);
+		break;
+	case LOSTFRAMING:
+		mISDN_FsmEvent(&l1->l1m, EV_ANYSIG_IND, NULL);
+		break;
+	case INFO2:
+		mISDN_FsmEvent(&l1->l1m, EV_INFO2_IND, NULL);
+		break;
+	case INFO4_P8:
+		mISDN_FsmEvent(&l1->l1m, EV_INFO4_IND, NULL);
+		break;
+	case INFO4_P10:
+		mISDN_FsmEvent(&l1->l1m, EV_INFO4_IND, NULL);
+		break;
+	case PH_ACTIVATE_REQ:
+		if (test_bit(FLG_L1_ACTIVATED, &l1->Flags))
+			l1->dcb(l1->dch, PH_ACTIVATE_IND);
+		else {
+			test_and_set_bit(FLG_L1_ACTIVATING, &l1->Flags);
+			mISDN_FsmEvent(&l1->l1m, EV_PH_ACTIVATE, NULL);
+		}
+		break;
+	case CLOSE_CHANNEL:
+		release_l1(l1);
+		break;
+	default:
+		if (*debug & DEBUG_L1)
+			printk(KERN_DEBUG "%s %x unhandled\n",
+			    __func__, event);
+		err = -EINVAL;
+	}
+	return err;
+}
+EXPORT_SYMBOL(l1_event);
+
+int
+create_l1(struct dchannel *dch, dchannel_l1callback *dcb) {
+	struct layer1	*nl1;
+
+	nl1 = kzalloc(sizeof(struct layer1), GFP_ATOMIC);
+	if (!nl1) {
+		printk(KERN_ERR "kmalloc struct layer1 failed\n");
+		return -ENOMEM;
+	}
+	nl1->l1m.fsm = &l1fsm_s;
+	nl1->l1m.state = ST_L1_F3;
+	nl1->Flags = 0;
+	nl1->l1m.debug = *debug & DEBUG_L1_FSM;
+	nl1->l1m.userdata = nl1;
+	nl1->l1m.userint = 0;
+	nl1->l1m.printdebug = l1m_debug;
+	nl1->dch = dch;
+	nl1->dcb = dcb;
+	mISDN_FsmInitTimer(&nl1->l1m, &nl1->timer);
+	__module_get(THIS_MODULE);
+	dch->l1 = nl1;
+	return 0;
+}
+EXPORT_SYMBOL(create_l1);
+
+int
+l1_init(u_int *deb)
+{
+	debug = deb;
+	l1fsm_s.state_count = L1S_STATE_COUNT;
+	l1fsm_s.event_count = L1_EVENT_COUNT;
+	l1fsm_s.strEvent = strL1Event;
+	l1fsm_s.strState = strL1SState;
+	mISDN_FsmNew(&l1fsm_s, L1SFnList, ARRAY_SIZE(L1SFnList));
+	return 0;
+}
+
+void
+l1_cleanup(void)
+{
+	mISDN_FsmFree(&l1fsm_s);
+}
diff --git a/drivers/isdn/mISDN/layer1.h b/drivers/isdn/mISDN/layer1.h
new file mode 100644
index 00000000000..9c8125fd89a
--- /dev/null
+++ b/drivers/isdn/mISDN/layer1.h
@@ -0,0 +1,26 @@
+/*
+ *
+ * Layer 1 defines
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.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.
+ *
+ */
+
+#define FLG_L1_ACTIVATING	1
+#define FLG_L1_ACTIVATED	2
+#define FLG_L1_DEACTTIMER	3
+#define FLG_L1_ACTTIMER		4
+#define FLG_L1_T3RUN		5
+#define FLG_L1_PULL_REQ		6
+#define FLG_L1_UINT		7
+#define FLG_L1_DBLOCKED		8
+
diff --git a/drivers/isdn/mISDN/layer2.c b/drivers/isdn/mISDN/layer2.c
new file mode 100644
index 00000000000..f5ad888ee71
--- /dev/null
+++ b/drivers/isdn/mISDN/layer2.c
@@ -0,0 +1,2216 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include "fsm.h"
+#include "layer2.h"
+
+static int *debug;
+
+static
+struct Fsm l2fsm = {NULL, 0, 0, NULL, NULL};
+
+static char *strL2State[] =
+{
+	"ST_L2_1",
+	"ST_L2_2",
+	"ST_L2_3",
+	"ST_L2_4",
+	"ST_L2_5",
+	"ST_L2_6",
+	"ST_L2_7",
+	"ST_L2_8",
+};
+
+enum {
+	EV_L2_UI,
+	EV_L2_SABME,
+	EV_L2_DISC,
+	EV_L2_DM,
+	EV_L2_UA,
+	EV_L2_FRMR,
+	EV_L2_SUPER,
+	EV_L2_I,
+	EV_L2_DL_DATA,
+	EV_L2_ACK_PULL,
+	EV_L2_DL_UNITDATA,
+	EV_L2_DL_ESTABLISH_REQ,
+	EV_L2_DL_RELEASE_REQ,
+	EV_L2_MDL_ASSIGN,
+	EV_L2_MDL_REMOVE,
+	EV_L2_MDL_ERROR,
+	EV_L1_DEACTIVATE,
+	EV_L2_T200,
+	EV_L2_T203,
+	EV_L2_SET_OWN_BUSY,
+	EV_L2_CLEAR_OWN_BUSY,
+	EV_L2_FRAME_ERROR,
+};
+
+#define L2_EVENT_COUNT (EV_L2_FRAME_ERROR+1)
+
+static char *strL2Event[] =
+{
+	"EV_L2_UI",
+	"EV_L2_SABME",
+	"EV_L2_DISC",
+	"EV_L2_DM",
+	"EV_L2_UA",
+	"EV_L2_FRMR",
+	"EV_L2_SUPER",
+	"EV_L2_I",
+	"EV_L2_DL_DATA",
+	"EV_L2_ACK_PULL",
+	"EV_L2_DL_UNITDATA",
+	"EV_L2_DL_ESTABLISH_REQ",
+	"EV_L2_DL_RELEASE_REQ",
+	"EV_L2_MDL_ASSIGN",
+	"EV_L2_MDL_REMOVE",
+	"EV_L2_MDL_ERROR",
+	"EV_L1_DEACTIVATE",
+	"EV_L2_T200",
+	"EV_L2_T203",
+	"EV_L2_SET_OWN_BUSY",
+	"EV_L2_CLEAR_OWN_BUSY",
+	"EV_L2_FRAME_ERROR",
+};
+
+static void
+l2m_debug(struct FsmInst *fi, char *fmt, ...)
+{
+	struct layer2 *l2 = fi->userdata;
+	va_list va;
+
+	if (!(*debug & DEBUG_L2_FSM))
+		return;
+	va_start(va, fmt);
+	printk(KERN_DEBUG "l2 (tei %d): ", l2->tei);
+	vprintk(fmt, va);
+	printk("\n");
+	va_end(va);
+}
+
+inline u_int
+l2headersize(struct layer2 *l2, int ui)
+{
+	return ((test_bit(FLG_MOD128, &l2->flag) && (!ui)) ? 2 : 1) +
+		(test_bit(FLG_LAPD, &l2->flag) ? 2 : 1);
+}
+
+inline u_int
+l2addrsize(struct layer2 *l2)
+{
+	return test_bit(FLG_LAPD, &l2->flag) ? 2 : 1;
+}
+
+static u_int
+l2_newid(struct layer2 *l2)
+{
+	u_int	id;
+
+	id = l2->next_id++;
+	if (id == 0x7fff)
+		l2->next_id = 1;
+	id <<= 16;
+	id |= l2->tei << 8;
+	id |= l2->sapi;
+	return id;
+}
+
+static void
+l2up(struct layer2 *l2, u_int prim, struct sk_buff *skb)
+{
+	int	err;
+
+	if (!l2->up)
+		return;
+	mISDN_HEAD_PRIM(skb) = prim;
+	mISDN_HEAD_ID(skb) = (l2->ch.nr << 16) | l2->ch.addr;
+	err = l2->up->send(l2->up, skb);
+	if (err) {
+		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
+		dev_kfree_skb(skb);
+	}
+}
+
+static void
+l2up_create(struct layer2 *l2, u_int prim, int len, void *arg)
+{
+	struct sk_buff	*skb;
+	struct mISDNhead *hh;
+	int		err;
+
+	if (!l2->up)
+		return;
+	skb = mI_alloc_skb(len, GFP_ATOMIC);
+	if (!skb)
+		return;
+	hh = mISDN_HEAD_P(skb);
+	hh->prim = prim;
+	hh->id = (l2->ch.nr << 16) | l2->ch.addr;
+	if (len)
+		memcpy(skb_put(skb, len), arg, len);
+	err = l2->up->send(l2->up, skb);
+	if (err) {
+		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
+		dev_kfree_skb(skb);
+	}
+}
+
+static int
+l2down_skb(struct layer2 *l2, struct sk_buff *skb) {
+	int ret;
+
+	ret = l2->ch.recv(l2->ch.peer, skb);
+	if (ret && (*debug & DEBUG_L2_RECV))
+		printk(KERN_DEBUG "l2down_skb: ret(%d)\n", ret);
+	return ret;
+}
+
+static int
+l2down_raw(struct layer2 *l2, struct sk_buff *skb)
+{
+	struct mISDNhead *hh = mISDN_HEAD_P(skb);
+
+	if (hh->prim == PH_DATA_REQ) {
+		if (test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) {
+			skb_queue_tail(&l2->down_queue, skb);
+			return 0;
+		}
+		l2->down_id = mISDN_HEAD_ID(skb);
+	}
+	return l2down_skb(l2, skb);
+}
+
+static int
+l2down(struct layer2 *l2, u_int prim, u_int id, struct sk_buff *skb)
+{
+	struct mISDNhead *hh = mISDN_HEAD_P(skb);
+
+	hh->prim = prim;
+	hh->id = id;
+	return l2down_raw(l2, skb);
+}
+
+static int
+l2down_create(struct layer2 *l2, u_int prim, u_int id, int len, void *arg)
+{
+	struct sk_buff	*skb;
+	int		err;
+	struct mISDNhead *hh;
+
+	skb = mI_alloc_skb(len, GFP_ATOMIC);
+	if (!skb)
+		return -ENOMEM;
+	hh = mISDN_HEAD_P(skb);
+	hh->prim = prim;
+	hh->id = id;
+	if (len)
+		memcpy(skb_put(skb, len), arg, len);
+	err = l2down_raw(l2, skb);
+	if (err)
+		dev_kfree_skb(skb);
+	return err;
+}
+
+static int
+ph_data_confirm(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb) {
+	struct sk_buff *nskb = skb;
+	int ret = -EAGAIN;
+
+	if (test_bit(FLG_L1_NOTREADY, &l2->flag)) {
+		if (hh->id == l2->down_id) {
+			nskb = skb_dequeue(&l2->down_queue);
+			if (nskb) {
+				l2->down_id = mISDN_HEAD_ID(nskb);
+				if (l2down_skb(l2, nskb)) {
+					dev_kfree_skb(nskb);
+					l2->down_id = MISDN_ID_NONE;
+				}
+			} else
+				l2->down_id = MISDN_ID_NONE;
+			if (ret) {
+				dev_kfree_skb(skb);
+				ret = 0;
+			}
+			if (l2->down_id == MISDN_ID_NONE) {
+				test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
+				mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL);
+			}
+		}
+	}
+	if (!test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) {
+		nskb = skb_dequeue(&l2->down_queue);
+		if (nskb) {
+			l2->down_id = mISDN_HEAD_ID(nskb);
+			if (l2down_skb(l2, nskb)) {
+				dev_kfree_skb(nskb);
+				l2->down_id = MISDN_ID_NONE;
+				test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
+			}
+		} else
+			test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
+	}
+	return ret;
+}
+
+static int
+l2mgr(struct layer2 *l2, u_int prim, void *arg) {
+	long c = (long)arg;
+
+	printk(KERN_WARNING
+	    "l2mgr: addr:%x prim %x %c\n", l2->id, prim, (char)c);
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		!test_bit(FLG_FIXED_TEI, &l2->flag)) {
+		switch (c) {
+		case 'C':
+		case 'D':
+		case 'G':
+		case 'H':
+			l2_tei(l2, prim, (u_long)arg);
+			break;
+		}
+	}
+	return 0;
+}
+
+static void
+set_peer_busy(struct layer2 *l2) {
+	test_and_set_bit(FLG_PEER_BUSY, &l2->flag);
+	if (skb_queue_len(&l2->i_queue) || skb_queue_len(&l2->ui_queue))
+		test_and_set_bit(FLG_L2BLOCK, &l2->flag);
+}
+
+static void
+clear_peer_busy(struct layer2 *l2) {
+	if (test_and_clear_bit(FLG_PEER_BUSY, &l2->flag))
+		test_and_clear_bit(FLG_L2BLOCK, &l2->flag);
+}
+
+static void
+InitWin(struct layer2 *l2)
+{
+	int i;
+
+	for (i = 0; i < MAX_WINDOW; i++)
+		l2->windowar[i] = NULL;
+}
+
+static int
+freewin(struct layer2 *l2)
+{
+	int i, cnt = 0;
+
+	for (i = 0; i < MAX_WINDOW; i++) {
+		if (l2->windowar[i]) {
+			cnt++;
+			dev_kfree_skb(l2->windowar[i]);
+			l2->windowar[i] = NULL;
+		}
+	}
+	return cnt;
+}
+
+static void
+ReleaseWin(struct layer2 *l2)
+{
+	int cnt = freewin(l2);
+
+	if (cnt)
+		printk(KERN_WARNING
+		    "isdnl2 freed %d skbuffs in release\n", cnt);
+}
+
+inline unsigned int
+cansend(struct layer2 *l2)
+{
+	unsigned int p1;
+
+	if (test_bit(FLG_MOD128, &l2->flag))
+		p1 = (l2->vs - l2->va) % 128;
+	else
+		p1 = (l2->vs - l2->va) % 8;
+	return (p1 < l2->window) && !test_bit(FLG_PEER_BUSY, &l2->flag);
+}
+
+inline void
+clear_exception(struct layer2 *l2)
+{
+	test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+	test_and_clear_bit(FLG_REJEXC, &l2->flag);
+	test_and_clear_bit(FLG_OWN_BUSY, &l2->flag);
+	clear_peer_busy(l2);
+}
+
+static int
+sethdraddr(struct layer2 *l2, u_char *header, int rsp)
+{
+	u_char *ptr = header;
+	int crbit = rsp;
+
+	if (test_bit(FLG_LAPD, &l2->flag)) {
+		if (test_bit(FLG_LAPD_NET, &l2->flag))
+			crbit = !crbit;
+		*ptr++ = (l2->sapi << 2) | (crbit ? 2 : 0);
+		*ptr++ = (l2->tei << 1) | 1;
+		return 2;
+	} else {
+		if (test_bit(FLG_ORIG, &l2->flag))
+			crbit = !crbit;
+		if (crbit)
+			*ptr++ = l2->addr.B;
+		else
+			*ptr++ = l2->addr.A;
+		return 1;
+	}
+}
+
+static inline void
+enqueue_super(struct layer2 *l2, struct sk_buff *skb)
+{
+	if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb))
+		dev_kfree_skb(skb);
+}
+
+static inline void
+enqueue_ui(struct layer2 *l2, struct sk_buff *skb)
+{
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_UI_IND, 0);
+	if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb))
+		dev_kfree_skb(skb);
+}
+
+inline int
+IsUI(u_char *data)
+{
+	return (data[0] & 0xef) == UI;
+}
+
+inline int
+IsUA(u_char *data)
+{
+	return (data[0] & 0xef) == UA;
+}
+
+inline int
+IsDM(u_char *data)
+{
+	return (data[0] & 0xef) == DM;
+}
+
+inline int
+IsDISC(u_char *data)
+{
+	return (data[0] & 0xef) == DISC;
+}
+
+inline int
+IsRR(u_char *data, struct layer2 *l2)
+{
+	if (test_bit(FLG_MOD128, &l2->flag))
+		return data[0] == RR;
+	else
+		return (data[0] & 0xf) == 1;
+}
+
+inline int
+IsSFrame(u_char *data, struct layer2 *l2)
+{
+	register u_char d = *data;
+
+	if (!test_bit(FLG_MOD128, &l2->flag))
+		d &= 0xf;
+	return ((d & 0xf3) == 1) && ((d & 0x0c) != 0x0c);
+}
+
+inline int
+IsSABME(u_char *data, struct layer2 *l2)
+{
+	u_char d = data[0] & ~0x10;
+
+	return test_bit(FLG_MOD128, &l2->flag) ? d == SABME : d == SABM;
+}
+
+inline int
+IsREJ(u_char *data, struct layer2 *l2)
+{
+	return test_bit(FLG_MOD128, &l2->flag) ?
+		data[0] == REJ : (data[0] & 0xf) == REJ;
+}
+
+inline int
+IsFRMR(u_char *data)
+{
+	return (data[0] & 0xef) == FRMR;
+}
+
+inline int
+IsRNR(u_char *data, struct layer2 *l2)
+{
+	return test_bit(FLG_MOD128, &l2->flag) ?
+	    data[0] == RNR : (data[0] & 0xf) == RNR;
+}
+
+int
+iframe_error(struct layer2 *l2, struct sk_buff *skb)
+{
+	u_int	i;
+	int	rsp = *skb->data & 0x2;
+
+	i = l2addrsize(l2) + (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1);
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+	if (rsp)
+		return 'L';
+	if (skb->len < i)
+		return 'N';
+	if ((skb->len - i) > l2->maxlen)
+		return 'O';
+	return 0;
+}
+
+int
+super_error(struct layer2 *l2, struct sk_buff *skb)
+{
+	if (skb->len != l2addrsize(l2) +
+	    (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1))
+		return 'N';
+	return 0;
+}
+
+int
+unnum_error(struct layer2 *l2, struct sk_buff *skb, int wantrsp)
+{
+	int rsp = (*skb->data & 0x2) >> 1;
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+	if (rsp != wantrsp)
+		return 'L';
+	if (skb->len != l2addrsize(l2) + 1)
+		return 'N';
+	return 0;
+}
+
+int
+UI_error(struct layer2 *l2, struct sk_buff *skb)
+{
+	int rsp = *skb->data & 0x2;
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+	if (rsp)
+		return 'L';
+	if (skb->len > l2->maxlen + l2addrsize(l2) + 1)
+		return 'O';
+	return 0;
+}
+
+int
+FRMR_error(struct layer2 *l2, struct sk_buff *skb)
+{
+	u_int	headers = l2addrsize(l2) + 1;
+	u_char	*datap = skb->data + headers;
+	int	rsp = *skb->data & 0x2;
+
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+	if (!rsp)
+		return 'L';
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		if (skb->len < headers + 5)
+			return 'N';
+		else if (*debug & DEBUG_L2)
+			l2m_debug(&l2->l2m,
+			    "FRMR information %2x %2x %2x %2x %2x",
+			    datap[0], datap[1], datap[2], datap[3], datap[4]);
+	} else {
+		if (skb->len < headers + 3)
+			return 'N';
+		else if (*debug & DEBUG_L2)
+			l2m_debug(&l2->l2m,
+			    "FRMR information %2x %2x %2x",
+			    datap[0], datap[1], datap[2]);
+	}
+	return 0;
+}
+
+static unsigned int
+legalnr(struct layer2 *l2, unsigned int nr)
+{
+	if (test_bit(FLG_MOD128, &l2->flag))
+		return ((nr - l2->va) % 128) <= ((l2->vs - l2->va) % 128);
+	else
+		return ((nr - l2->va) % 8) <= ((l2->vs - l2->va) % 8);
+}
+
+static void
+setva(struct layer2 *l2, unsigned int nr)
+{
+	struct sk_buff	*skb;
+
+	while (l2->va != nr) {
+		l2->va++;
+		if (test_bit(FLG_MOD128, &l2->flag))
+			l2->va %= 128;
+		else
+			l2->va %= 8;
+		if (l2->windowar[l2->sow]) {
+			skb_trim(l2->windowar[l2->sow], 0);
+			skb_queue_tail(&l2->tmp_queue, l2->windowar[l2->sow]);
+			l2->windowar[l2->sow] = NULL;
+		}
+		l2->sow = (l2->sow + 1) % l2->window;
+	}
+	skb = skb_dequeue(&l2->tmp_queue);
+	while (skb) {
+		dev_kfree_skb(skb);
+		skb = skb_dequeue(&l2->tmp_queue);
+	}
+}
+
+static void
+send_uframe(struct layer2 *l2, struct sk_buff *skb, u_char cmd, u_char cr)
+{
+	u_char tmp[MAX_L2HEADER_LEN];
+	int i;
+
+	i = sethdraddr(l2, tmp, cr);
+	tmp[i++] = cmd;
+	if (skb)
+		skb_trim(skb, 0);
+	else {
+		skb = mI_alloc_skb(i, GFP_ATOMIC);
+		if (!skb) {
+			printk(KERN_WARNING "%s: can't alloc skbuff\n",
+				__func__);
+			return;
+		}
+	}
+	memcpy(skb_put(skb, i), tmp, i);
+	enqueue_super(l2, skb);
+}
+
+
+inline u_char
+get_PollFlag(struct layer2 *l2, struct sk_buff *skb)
+{
+	return skb->data[l2addrsize(l2)] & 0x10;
+}
+
+inline u_char
+get_PollFlagFree(struct layer2 *l2, struct sk_buff *skb)
+{
+	u_char PF;
+
+	PF = get_PollFlag(l2, skb);
+	dev_kfree_skb(skb);
+	return PF;
+}
+
+inline void
+start_t200(struct layer2 *l2, int i)
+{
+	mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i);
+	test_and_set_bit(FLG_T200_RUN, &l2->flag);
+}
+
+inline void
+restart_t200(struct layer2 *l2, int i)
+{
+	mISDN_FsmRestartTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i);
+	test_and_set_bit(FLG_T200_RUN, &l2->flag);
+}
+
+inline void
+stop_t200(struct layer2 *l2, int i)
+{
+	if (test_and_clear_bit(FLG_T200_RUN, &l2->flag))
+		mISDN_FsmDelTimer(&l2->t200, i);
+}
+
+inline void
+st5_dl_release_l2l3(struct layer2 *l2)
+{
+	int pr;
+
+	if (test_and_clear_bit(FLG_PEND_REL, &l2->flag))
+		pr = DL_RELEASE_CNF;
+	else
+		pr = DL_RELEASE_IND;
+	l2up_create(l2, pr, 0, NULL);
+}
+
+inline void
+lapb_dl_release_l2l3(struct layer2 *l2, int f)
+{
+	if (test_bit(FLG_LAPB, &l2->flag))
+		l2down_create(l2, PH_DEACTIVATE_REQ, l2_newid(l2), 0, NULL);
+	l2up_create(l2, f, 0, NULL);
+}
+
+static void
+establishlink(struct FsmInst *fi)
+{
+	struct layer2 *l2 = fi->userdata;
+	u_char cmd;
+
+	clear_exception(l2);
+	l2->rc = 0;
+	cmd = (test_bit(FLG_MOD128, &l2->flag) ? SABME : SABM) | 0x10;
+	send_uframe(l2, NULL, cmd, CMD);
+	mISDN_FsmDelTimer(&l2->t203, 1);
+	restart_t200(l2, 1);
+	test_and_clear_bit(FLG_PEND_REL, &l2->flag);
+	freewin(l2);
+	mISDN_FsmChangeState(fi, ST_L2_5);
+}
+
+static void
+l2_mdl_error_ua(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	if (get_PollFlagFree(l2, skb))
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'C');
+	else
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'D');
+
+}
+
+static void
+l2_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	if (get_PollFlagFree(l2, skb))
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'B');
+	else {
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'E');
+		establishlink(fi);
+		test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+	}
+}
+
+static void
+l2_st8_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	if (get_PollFlagFree(l2, skb))
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'B');
+	else
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'E');
+	establishlink(fi);
+	test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+}
+
+static void
+l2_go_st3(struct FsmInst *fi, int event, void *arg)
+{
+	dev_kfree_skb((struct sk_buff *)arg);
+	mISDN_FsmChangeState(fi, ST_L2_3);
+}
+
+static void
+l2_mdl_assign(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+
+	mISDN_FsmChangeState(fi, ST_L2_3);
+	dev_kfree_skb((struct sk_buff *)arg);
+	l2_tei(l2, MDL_ASSIGN_IND, 0);
+}
+
+static void
+l2_queue_ui_assign(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_tail(&l2->ui_queue, skb);
+	mISDN_FsmChangeState(fi, ST_L2_2);
+	l2_tei(l2, MDL_ASSIGN_IND, 0);
+}
+
+static void
+l2_queue_ui(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_tail(&l2->ui_queue, skb);
+}
+
+static void
+tx_ui(struct layer2 *l2)
+{
+	struct sk_buff *skb;
+	u_char header[MAX_L2HEADER_LEN];
+	int i;
+
+	i = sethdraddr(l2, header, CMD);
+	if (test_bit(FLG_LAPD_NET, &l2->flag))
+		header[1] = 0xff; /* tei 127 */
+	header[i++] = UI;
+	while ((skb = skb_dequeue(&l2->ui_queue))) {
+		memcpy(skb_push(skb, i), header, i);
+		enqueue_ui(l2, skb);
+	}
+}
+
+static void
+l2_send_ui(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_tail(&l2->ui_queue, skb);
+	tx_ui(l2);
+}
+
+static void
+l2_got_ui(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_pull(skb, l2headersize(l2, 1));
+/*
+ *		in states 1-3 for broadcast
+ */
+
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_UI_IND, 0);
+	l2up(l2, DL_UNITDATA_IND, skb);
+}
+
+static void
+l2_establish(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	establishlink(fi);
+	test_and_set_bit(FLG_L3_INIT, &l2->flag);
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_discard_i_setl3(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->i_queue);
+	test_and_set_bit(FLG_L3_INIT, &l2->flag);
+	test_and_clear_bit(FLG_PEND_REL, &l2->flag);
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_l3_reestablish(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->i_queue);
+	establishlink(fi);
+	test_and_set_bit(FLG_L3_INIT, &l2->flag);
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_release(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_trim(skb, 0);
+	l2up(l2, DL_RELEASE_CNF, skb);
+}
+
+static void
+l2_pend_rel(struct FsmInst *fi, int event, void *arg)
+{
+	struct sk_buff *skb = arg;
+	struct layer2 *l2 = fi->userdata;
+
+	test_and_set_bit(FLG_PEND_REL, &l2->flag);
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_disconnect(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_purge(&l2->i_queue);
+	freewin(l2);
+	mISDN_FsmChangeState(fi, ST_L2_6);
+	l2->rc = 0;
+	send_uframe(l2, NULL, DISC | 0x10, CMD);
+	mISDN_FsmDelTimer(&l2->t203, 1);
+	restart_t200(l2, 2);
+	if (skb)
+		dev_kfree_skb(skb);
+}
+
+static void
+l2_start_multi(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb = arg;
+
+	l2->vs = 0;
+	l2->va = 0;
+	l2->vr = 0;
+	l2->sow = 0;
+	clear_exception(l2);
+	send_uframe(l2, NULL, UA | get_PollFlag(l2, skb), RSP);
+	mISDN_FsmChangeState(fi, ST_L2_7);
+	mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3);
+	skb_trim(skb, 0);
+	l2up(l2, DL_ESTABLISH_IND, skb);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_UP_IND, 0);
+}
+
+static void
+l2_send_UA(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
+}
+
+static void
+l2_send_DM(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	send_uframe(l2, skb, DM | get_PollFlag(l2, skb), RSP);
+}
+
+static void
+l2_restart_multi(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb = arg;
+	int		est = 0;
+
+	send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
+
+	l2mgr(l2, MDL_ERROR_IND, (void *) 'F');
+
+	if (l2->vs != l2->va) {
+		skb_queue_purge(&l2->i_queue);
+		est = 1;
+	}
+
+	clear_exception(l2);
+	l2->vs = 0;
+	l2->va = 0;
+	l2->vr = 0;
+	l2->sow = 0;
+	mISDN_FsmChangeState(fi, ST_L2_7);
+	stop_t200(l2, 3);
+	mISDN_FsmRestartTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3);
+
+	if (est)
+		l2up_create(l2, DL_ESTABLISH_IND, 0, NULL);
+/*		mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST,
+ *		    MGR_SHORTSTATUS | INDICATION, SSTATUS_L2_ESTABLISHED,
+ *		    0, NULL, 0);
+ */
+	if (skb_queue_len(&l2->i_queue) && cansend(l2))
+		mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+}
+
+static void
+l2_stop_multi(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb = arg;
+
+	mISDN_FsmChangeState(fi, ST_L2_4);
+	mISDN_FsmDelTimer(&l2->t203, 3);
+	stop_t200(l2, 4);
+
+	send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
+	skb_queue_purge(&l2->i_queue);
+	freewin(l2);
+	lapb_dl_release_l2l3(l2, DL_RELEASE_IND);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+}
+
+static void
+l2_connected(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb = arg;
+	int pr = -1;
+
+	if (!get_PollFlag(l2, skb)) {
+		l2_mdl_error_ua(fi, event, arg);
+		return;
+	}
+	dev_kfree_skb(skb);
+	if (test_and_clear_bit(FLG_PEND_REL, &l2->flag))
+		l2_disconnect(fi, event, NULL);
+	if (test_and_clear_bit(FLG_L3_INIT, &l2->flag)) {
+		pr = DL_ESTABLISH_CNF;
+	} else if (l2->vs != l2->va) {
+		skb_queue_purge(&l2->i_queue);
+		pr = DL_ESTABLISH_IND;
+	}
+	stop_t200(l2, 5);
+	l2->vr = 0;
+	l2->vs = 0;
+	l2->va = 0;
+	l2->sow = 0;
+	mISDN_FsmChangeState(fi, ST_L2_7);
+	mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 4);
+	if (pr != -1)
+		l2up_create(l2, pr, 0, NULL);
+
+	if (skb_queue_len(&l2->i_queue) && cansend(l2))
+		mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_UP_IND, 0);
+}
+
+static void
+l2_released(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (!get_PollFlag(l2, skb)) {
+		l2_mdl_error_ua(fi, event, arg);
+		return;
+	}
+	dev_kfree_skb(skb);
+	stop_t200(l2, 6);
+	lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
+	mISDN_FsmChangeState(fi, ST_L2_4);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+}
+
+static void
+l2_reestablish(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (!get_PollFlagFree(l2, skb)) {
+		establishlink(fi);
+		test_and_set_bit(FLG_L3_INIT, &l2->flag);
+	}
+}
+
+static void
+l2_st5_dm_release(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (get_PollFlagFree(l2, skb)) {
+		stop_t200(l2, 7);
+		if (!test_bit(FLG_L3_INIT, &l2->flag))
+			skb_queue_purge(&l2->i_queue);
+		if (test_bit(FLG_LAPB, &l2->flag))
+			l2down_create(l2, PH_DEACTIVATE_REQ,
+				l2_newid(l2), 0, NULL);
+		st5_dl_release_l2l3(l2);
+		mISDN_FsmChangeState(fi, ST_L2_4);
+		if (l2->tm)
+			l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+	}
+}
+
+static void
+l2_st6_dm_release(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (get_PollFlagFree(l2, skb)) {
+		stop_t200(l2, 8);
+		lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
+		mISDN_FsmChangeState(fi, ST_L2_4);
+		if (l2->tm)
+			l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+	}
+}
+
+void
+enquiry_cr(struct layer2 *l2, u_char typ, u_char cr, u_char pf)
+{
+	struct sk_buff *skb;
+	u_char tmp[MAX_L2HEADER_LEN];
+	int i;
+
+	i = sethdraddr(l2, tmp, cr);
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		tmp[i++] = typ;
+		tmp[i++] = (l2->vr << 1) | (pf ? 1 : 0);
+	} else
+		tmp[i++] = (l2->vr << 5) | typ | (pf ? 0x10 : 0);
+	skb = mI_alloc_skb(i, GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_WARNING
+		    "isdnl2 can't alloc sbbuff for enquiry_cr\n");
+		return;
+	}
+	memcpy(skb_put(skb, i), tmp, i);
+	enqueue_super(l2, skb);
+}
+
+inline void
+enquiry_response(struct layer2 *l2)
+{
+	if (test_bit(FLG_OWN_BUSY, &l2->flag))
+		enquiry_cr(l2, RNR, RSP, 1);
+	else
+		enquiry_cr(l2, RR, RSP, 1);
+	test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+}
+
+inline void
+transmit_enquiry(struct layer2 *l2)
+{
+	if (test_bit(FLG_OWN_BUSY, &l2->flag))
+		enquiry_cr(l2, RNR, CMD, 1);
+	else
+		enquiry_cr(l2, RR, CMD, 1);
+	test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+	start_t200(l2, 9);
+}
+
+
+static void
+nrerrorrecovery(struct FsmInst *fi)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	l2mgr(l2, MDL_ERROR_IND, (void *) 'J');
+	establishlink(fi);
+	test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+}
+
+static void
+invoke_retransmission(struct layer2 *l2, unsigned int nr)
+{
+	u_int	p1;
+
+	if (l2->vs != nr) {
+		while (l2->vs != nr) {
+			(l2->vs)--;
+			if (test_bit(FLG_MOD128, &l2->flag)) {
+				l2->vs %= 128;
+				p1 = (l2->vs - l2->va) % 128;
+			} else {
+				l2->vs %= 8;
+				p1 = (l2->vs - l2->va) % 8;
+			}
+			p1 = (p1 + l2->sow) % l2->window;
+			if (l2->windowar[p1])
+				skb_queue_head(&l2->i_queue, l2->windowar[p1]);
+			else
+				printk(KERN_WARNING
+				    "%s: windowar[%d] is NULL\n",
+				    __func__, p1);
+			l2->windowar[p1] = NULL;
+		}
+		mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL);
+	}
+}
+
+static void
+l2_st7_got_super(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+	int PollFlag, rsp, typ = RR;
+	unsigned int nr;
+
+	rsp = *skb->data & 0x2;
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+
+	skb_pull(skb, l2addrsize(l2));
+	if (IsRNR(skb->data, l2)) {
+		set_peer_busy(l2);
+		typ = RNR;
+	} else
+		clear_peer_busy(l2);
+	if (IsREJ(skb->data, l2))
+		typ = REJ;
+
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		PollFlag = (skb->data[1] & 0x1) == 0x1;
+		nr = skb->data[1] >> 1;
+	} else {
+		PollFlag = (skb->data[0] & 0x10);
+		nr = (skb->data[0] >> 5) & 0x7;
+	}
+	dev_kfree_skb(skb);
+
+	if (PollFlag) {
+		if (rsp)
+			l2mgr(l2, MDL_ERROR_IND, (void *) 'A');
+		else
+			enquiry_response(l2);
+	}
+	if (legalnr(l2, nr)) {
+		if (typ == REJ) {
+			setva(l2, nr);
+			invoke_retransmission(l2, nr);
+			stop_t200(l2, 10);
+			if (mISDN_FsmAddTimer(&l2->t203, l2->T203,
+					EV_L2_T203, NULL, 6))
+				l2m_debug(&l2->l2m, "Restart T203 ST7 REJ");
+		} else if ((nr == l2->vs) && (typ == RR)) {
+			setva(l2, nr);
+			stop_t200(l2, 11);
+			mISDN_FsmRestartTimer(&l2->t203, l2->T203,
+					EV_L2_T203, NULL, 7);
+		} else if ((l2->va != nr) || (typ == RNR)) {
+			setva(l2, nr);
+			if (typ != RR)
+				mISDN_FsmDelTimer(&l2->t203, 9);
+			restart_t200(l2, 12);
+		}
+		if (skb_queue_len(&l2->i_queue) && (typ == RR))
+			mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+	} else
+		nrerrorrecovery(fi);
+}
+
+static void
+l2_feed_i_if_reest(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (!test_bit(FLG_L3_INIT, &l2->flag))
+		skb_queue_tail(&l2->i_queue, skb);
+	else
+		dev_kfree_skb(skb);
+}
+
+static void
+l2_feed_i_pull(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_tail(&l2->i_queue, skb);
+	mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+}
+
+static void
+l2_feed_iqueue(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_tail(&l2->i_queue, skb);
+}
+
+static void
+l2_got_iframe(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb = arg;
+	int		PollFlag, i;
+	u_int		ns, nr;
+
+	i = l2addrsize(l2);
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		PollFlag = ((skb->data[i + 1] & 0x1) == 0x1);
+		ns = skb->data[i] >> 1;
+		nr = (skb->data[i + 1] >> 1) & 0x7f;
+	} else {
+		PollFlag = (skb->data[i] & 0x10);
+		ns = (skb->data[i] >> 1) & 0x7;
+		nr = (skb->data[i] >> 5) & 0x7;
+	}
+	if (test_bit(FLG_OWN_BUSY, &l2->flag)) {
+		dev_kfree_skb(skb);
+		if (PollFlag)
+			enquiry_response(l2);
+	} else {
+		if (l2->vr == ns) {
+			l2->vr++;
+			if (test_bit(FLG_MOD128, &l2->flag))
+				l2->vr %= 128;
+			else
+				l2->vr %= 8;
+			test_and_clear_bit(FLG_REJEXC, &l2->flag);
+			if (PollFlag)
+				enquiry_response(l2);
+			else
+				test_and_set_bit(FLG_ACK_PEND, &l2->flag);
+			skb_pull(skb, l2headersize(l2, 0));
+			l2up(l2, DL_DATA_IND, skb);
+		} else {
+			/* n(s)!=v(r) */
+			dev_kfree_skb(skb);
+			if (test_and_set_bit(FLG_REJEXC, &l2->flag)) {
+				if (PollFlag)
+					enquiry_response(l2);
+			} else {
+				enquiry_cr(l2, REJ, RSP, PollFlag);
+				test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+			}
+		}
+	}
+	if (legalnr(l2, nr)) {
+		if (!test_bit(FLG_PEER_BUSY, &l2->flag) &&
+		    (fi->state == ST_L2_7)) {
+			if (nr == l2->vs) {
+				stop_t200(l2, 13);
+				mISDN_FsmRestartTimer(&l2->t203, l2->T203,
+						EV_L2_T203, NULL, 7);
+			} else if (nr != l2->va)
+				restart_t200(l2, 14);
+		}
+		setva(l2, nr);
+	} else {
+		nrerrorrecovery(fi);
+		return;
+	}
+	if (skb_queue_len(&l2->i_queue) && (fi->state == ST_L2_7))
+		mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+	if (test_and_clear_bit(FLG_ACK_PEND, &l2->flag))
+		enquiry_cr(l2, RR, RSP, 0);
+}
+
+static void
+l2_got_tei(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	u_int		info;
+
+	l2->tei = (signed char)(long)arg;
+	set_channel_address(&l2->ch, l2->sapi, l2->tei);
+	info = DL_INFO_L2_CONNECT;
+	l2up_create(l2, DL_INFORMATION_IND, sizeof(info), &info);
+	if (fi->state == ST_L2_3) {
+		establishlink(fi);
+		test_and_set_bit(FLG_L3_INIT, &l2->flag);
+	} else
+		mISDN_FsmChangeState(fi, ST_L2_4);
+	if (skb_queue_len(&l2->ui_queue))
+		tx_ui(l2);
+}
+
+static void
+l2_st5_tout_200(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
+	} else if (l2->rc == l2->N200) {
+		mISDN_FsmChangeState(fi, ST_L2_4);
+		test_and_clear_bit(FLG_T200_RUN, &l2->flag);
+		skb_queue_purge(&l2->i_queue);
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'G');
+		if (test_bit(FLG_LAPB, &l2->flag))
+			l2down_create(l2, PH_DEACTIVATE_REQ,
+				l2_newid(l2), 0, NULL);
+		st5_dl_release_l2l3(l2);
+		if (l2->tm)
+			l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+	} else {
+		l2->rc++;
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
+		send_uframe(l2, NULL, (test_bit(FLG_MOD128, &l2->flag) ?
+			SABME : SABM) | 0x10, CMD);
+	}
+}
+
+static void
+l2_st6_tout_200(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
+	} else if (l2->rc == l2->N200) {
+		mISDN_FsmChangeState(fi, ST_L2_4);
+		test_and_clear_bit(FLG_T200_RUN, &l2->flag);
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'H');
+		lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
+		if (l2->tm)
+			l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+	} else {
+		l2->rc++;
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200,
+			    NULL, 9);
+		send_uframe(l2, NULL, DISC | 0x10, CMD);
+	}
+}
+
+static void
+l2_st7_tout_200(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
+		return;
+	}
+	test_and_clear_bit(FLG_T200_RUN, &l2->flag);
+	l2->rc = 0;
+	mISDN_FsmChangeState(fi, ST_L2_8);
+	transmit_enquiry(l2);
+	l2->rc++;
+}
+
+static void
+l2_st8_tout_200(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
+		return;
+	}
+	test_and_clear_bit(FLG_T200_RUN, &l2->flag);
+	if (l2->rc == l2->N200) {
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'I');
+		establishlink(fi);
+		test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+	} else {
+		transmit_enquiry(l2);
+		l2->rc++;
+	}
+}
+
+static void
+l2_st7_tout_203(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	if (test_bit(FLG_LAPD, &l2->flag) &&
+		test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
+		mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 9);
+		return;
+	}
+	mISDN_FsmChangeState(fi, ST_L2_8);
+	transmit_enquiry(l2);
+	l2->rc = 0;
+}
+
+static void
+l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2	*l2 = fi->userdata;
+	struct sk_buff	*skb, *nskb, *oskb;
+	u_char		header[MAX_L2HEADER_LEN];
+	u_int		i, p1;
+
+	if (!cansend(l2))
+		return;
+
+	skb = skb_dequeue(&l2->i_queue);
+	if (!skb)
+		return;
+
+	if (test_bit(FLG_MOD128, &l2->flag))
+		p1 = (l2->vs - l2->va) % 128;
+	else
+		p1 = (l2->vs - l2->va) % 8;
+	p1 = (p1 + l2->sow) % l2->window;
+	if (l2->windowar[p1]) {
+		printk(KERN_WARNING "isdnl2 try overwrite ack queue entry %d\n",
+		    p1);
+		dev_kfree_skb(l2->windowar[p1]);
+	}
+	l2->windowar[p1] = skb;
+	i = sethdraddr(l2, header, CMD);
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		header[i++] = l2->vs << 1;
+		header[i++] = l2->vr << 1;
+		l2->vs = (l2->vs + 1) % 128;
+	} else {
+		header[i++] = (l2->vr << 5) | (l2->vs << 1);
+		l2->vs = (l2->vs + 1) % 8;
+	}
+
+	nskb = skb_clone(skb, GFP_ATOMIC);
+	p1 = skb_headroom(nskb);
+	if (p1 >= i)
+		memcpy(skb_push(nskb, i), header, i);
+	else {
+		printk(KERN_WARNING
+		    "isdnl2 pull_iqueue skb header(%d/%d) too short\n", i, p1);
+		oskb = nskb;
+		nskb = mI_alloc_skb(oskb->len + i, GFP_ATOMIC);
+		if (!nskb) {
+			dev_kfree_skb(oskb);
+			printk(KERN_WARNING "%s: no skb mem\n", __func__);
+			return;
+		}
+		memcpy(skb_put(nskb, i), header, i);
+		memcpy(skb_put(nskb, oskb->len), oskb->data, oskb->len);
+		dev_kfree_skb(oskb);
+	}
+	l2down(l2, PH_DATA_REQ, l2_newid(l2), nskb);
+	test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+	if (!test_and_set_bit(FLG_T200_RUN, &l2->flag)) {
+		mISDN_FsmDelTimer(&l2->t203, 13);
+		mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 11);
+	}
+}
+
+static void
+l2_st8_got_super(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+	int PollFlag, rsp, rnr = 0;
+	unsigned int nr;
+
+	rsp = *skb->data & 0x2;
+	if (test_bit(FLG_ORIG, &l2->flag))
+		rsp = !rsp;
+
+	skb_pull(skb, l2addrsize(l2));
+
+	if (IsRNR(skb->data, l2)) {
+		set_peer_busy(l2);
+		rnr = 1;
+	} else
+		clear_peer_busy(l2);
+
+	if (test_bit(FLG_MOD128, &l2->flag)) {
+		PollFlag = (skb->data[1] & 0x1) == 0x1;
+		nr = skb->data[1] >> 1;
+	} else {
+		PollFlag = (skb->data[0] & 0x10);
+		nr = (skb->data[0] >> 5) & 0x7;
+	}
+	dev_kfree_skb(skb);
+	if (rsp && PollFlag) {
+		if (legalnr(l2, nr)) {
+			if (rnr) {
+				restart_t200(l2, 15);
+			} else {
+				stop_t200(l2, 16);
+				mISDN_FsmAddTimer(&l2->t203, l2->T203,
+					    EV_L2_T203, NULL, 5);
+				setva(l2, nr);
+			}
+			invoke_retransmission(l2, nr);
+			mISDN_FsmChangeState(fi, ST_L2_7);
+			if (skb_queue_len(&l2->i_queue) && cansend(l2))
+				mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
+		} else
+			nrerrorrecovery(fi);
+	} else {
+		if (!rsp && PollFlag)
+			enquiry_response(l2);
+		if (legalnr(l2, nr))
+			setva(l2, nr);
+		else
+			nrerrorrecovery(fi);
+	}
+}
+
+static void
+l2_got_FRMR(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_pull(skb, l2addrsize(l2) + 1);
+
+	if (!(skb->data[0] & 1) || ((skb->data[0] & 3) == 1) || /* I or S */
+	    (IsUA(skb->data) && (fi->state == ST_L2_7))) {
+		l2mgr(l2, MDL_ERROR_IND, (void *) 'K');
+		establishlink(fi);
+		test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+	}
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_st24_tei_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->ui_queue);
+	l2->tei = GROUP_TEI;
+	mISDN_FsmChangeState(fi, ST_L2_1);
+}
+
+static void
+l2_st3_tei_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->ui_queue);
+	l2->tei = GROUP_TEI;
+	l2up_create(l2, DL_RELEASE_IND, 0, NULL);
+	mISDN_FsmChangeState(fi, ST_L2_1);
+}
+
+static void
+l2_st5_tei_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	freewin(l2);
+	l2->tei = GROUP_TEI;
+	stop_t200(l2, 17);
+	st5_dl_release_l2l3(l2);
+	mISDN_FsmChangeState(fi, ST_L2_1);
+}
+
+static void
+l2_st6_tei_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->ui_queue);
+	l2->tei = GROUP_TEI;
+	stop_t200(l2, 18);
+	l2up_create(l2, DL_RELEASE_IND, 0, NULL);
+	mISDN_FsmChangeState(fi, ST_L2_1);
+}
+
+static void
+l2_tei_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	freewin(l2);
+	l2->tei = GROUP_TEI;
+	stop_t200(l2, 17);
+	mISDN_FsmDelTimer(&l2->t203, 19);
+	l2up_create(l2, DL_RELEASE_IND, 0, NULL);
+/*	mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST,
+ *		MGR_SHORTSTATUS_IND, SSTATUS_L2_RELEASED,
+ *		0, NULL, 0);
+ */
+	mISDN_FsmChangeState(fi, ST_L2_1);
+}
+
+static void
+l2_st14_persistant_da(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag))
+		l2up(l2, DL_RELEASE_IND, skb);
+	else
+		dev_kfree_skb(skb);
+}
+
+static void
+l2_st5_persistant_da(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	freewin(l2);
+	stop_t200(l2, 19);
+	st5_dl_release_l2l3(l2);
+	mISDN_FsmChangeState(fi, ST_L2_4);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+	dev_kfree_skb(skb);
+}
+
+static void
+l2_st6_persistant_da(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_purge(&l2->ui_queue);
+	stop_t200(l2, 20);
+	l2up(l2, DL_RELEASE_CNF, skb);
+	mISDN_FsmChangeState(fi, ST_L2_4);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+}
+
+static void
+l2_persistant_da(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	freewin(l2);
+	stop_t200(l2, 19);
+	mISDN_FsmDelTimer(&l2->t203, 19);
+	l2up(l2, DL_RELEASE_IND, skb);
+	mISDN_FsmChangeState(fi, ST_L2_4);
+	if (l2->tm)
+		l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
+}
+
+static void
+l2_set_own_busy(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (!test_and_set_bit(FLG_OWN_BUSY, &l2->flag)) {
+		enquiry_cr(l2, RNR, RSP, 0);
+		test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+	}
+	if (skb)
+		dev_kfree_skb(skb);
+}
+
+static void
+l2_clear_own_busy(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb = arg;
+
+	if (!test_and_clear_bit(FLG_OWN_BUSY, &l2->flag)) {
+		enquiry_cr(l2, RR, RSP, 0);
+		test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
+	}
+	if (skb)
+		dev_kfree_skb(skb);
+}
+
+static void
+l2_frame_error(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	l2mgr(l2, MDL_ERROR_IND, arg);
+}
+
+static void
+l2_frame_error_reest(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+
+	l2mgr(l2, MDL_ERROR_IND, arg);
+	establishlink(fi);
+	test_and_clear_bit(FLG_L3_INIT, &l2->flag);
+}
+
+static struct FsmNode L2FnList[] =
+{
+	{ST_L2_1, EV_L2_DL_ESTABLISH_REQ, l2_mdl_assign},
+	{ST_L2_2, EV_L2_DL_ESTABLISH_REQ, l2_go_st3},
+	{ST_L2_4, EV_L2_DL_ESTABLISH_REQ, l2_establish},
+	{ST_L2_5, EV_L2_DL_ESTABLISH_REQ, l2_discard_i_setl3},
+	{ST_L2_7, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
+	{ST_L2_8, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
+	{ST_L2_4, EV_L2_DL_RELEASE_REQ, l2_release},
+	{ST_L2_5, EV_L2_DL_RELEASE_REQ, l2_pend_rel},
+	{ST_L2_7, EV_L2_DL_RELEASE_REQ, l2_disconnect},
+	{ST_L2_8, EV_L2_DL_RELEASE_REQ, l2_disconnect},
+	{ST_L2_5, EV_L2_DL_DATA, l2_feed_i_if_reest},
+	{ST_L2_7, EV_L2_DL_DATA, l2_feed_i_pull},
+	{ST_L2_8, EV_L2_DL_DATA, l2_feed_iqueue},
+	{ST_L2_1, EV_L2_DL_UNITDATA, l2_queue_ui_assign},
+	{ST_L2_2, EV_L2_DL_UNITDATA, l2_queue_ui},
+	{ST_L2_3, EV_L2_DL_UNITDATA, l2_queue_ui},
+	{ST_L2_4, EV_L2_DL_UNITDATA, l2_send_ui},
+	{ST_L2_5, EV_L2_DL_UNITDATA, l2_send_ui},
+	{ST_L2_6, EV_L2_DL_UNITDATA, l2_send_ui},
+	{ST_L2_7, EV_L2_DL_UNITDATA, l2_send_ui},
+	{ST_L2_8, EV_L2_DL_UNITDATA, l2_send_ui},
+	{ST_L2_1, EV_L2_MDL_ASSIGN, l2_got_tei},
+	{ST_L2_2, EV_L2_MDL_ASSIGN, l2_got_tei},
+	{ST_L2_3, EV_L2_MDL_ASSIGN, l2_got_tei},
+	{ST_L2_2, EV_L2_MDL_ERROR, l2_st24_tei_remove},
+	{ST_L2_3, EV_L2_MDL_ERROR, l2_st3_tei_remove},
+	{ST_L2_4, EV_L2_MDL_REMOVE, l2_st24_tei_remove},
+	{ST_L2_5, EV_L2_MDL_REMOVE, l2_st5_tei_remove},
+	{ST_L2_6, EV_L2_MDL_REMOVE, l2_st6_tei_remove},
+	{ST_L2_7, EV_L2_MDL_REMOVE, l2_tei_remove},
+	{ST_L2_8, EV_L2_MDL_REMOVE, l2_tei_remove},
+	{ST_L2_4, EV_L2_SABME, l2_start_multi},
+	{ST_L2_5, EV_L2_SABME, l2_send_UA},
+	{ST_L2_6, EV_L2_SABME, l2_send_DM},
+	{ST_L2_7, EV_L2_SABME, l2_restart_multi},
+	{ST_L2_8, EV_L2_SABME, l2_restart_multi},
+	{ST_L2_4, EV_L2_DISC, l2_send_DM},
+	{ST_L2_5, EV_L2_DISC, l2_send_DM},
+	{ST_L2_6, EV_L2_DISC, l2_send_UA},
+	{ST_L2_7, EV_L2_DISC, l2_stop_multi},
+	{ST_L2_8, EV_L2_DISC, l2_stop_multi},
+	{ST_L2_4, EV_L2_UA, l2_mdl_error_ua},
+	{ST_L2_5, EV_L2_UA, l2_connected},
+	{ST_L2_6, EV_L2_UA, l2_released},
+	{ST_L2_7, EV_L2_UA, l2_mdl_error_ua},
+	{ST_L2_8, EV_L2_UA, l2_mdl_error_ua},
+	{ST_L2_4, EV_L2_DM, l2_reestablish},
+	{ST_L2_5, EV_L2_DM, l2_st5_dm_release},
+	{ST_L2_6, EV_L2_DM, l2_st6_dm_release},
+	{ST_L2_7, EV_L2_DM, l2_mdl_error_dm},
+	{ST_L2_8, EV_L2_DM, l2_st8_mdl_error_dm},
+	{ST_L2_1, EV_L2_UI, l2_got_ui},
+	{ST_L2_2, EV_L2_UI, l2_got_ui},
+	{ST_L2_3, EV_L2_UI, l2_got_ui},
+	{ST_L2_4, EV_L2_UI, l2_got_ui},
+	{ST_L2_5, EV_L2_UI, l2_got_ui},
+	{ST_L2_6, EV_L2_UI, l2_got_ui},
+	{ST_L2_7, EV_L2_UI, l2_got_ui},
+	{ST_L2_8, EV_L2_UI, l2_got_ui},
+	{ST_L2_7, EV_L2_FRMR, l2_got_FRMR},
+	{ST_L2_8, EV_L2_FRMR, l2_got_FRMR},
+	{ST_L2_7, EV_L2_SUPER, l2_st7_got_super},
+	{ST_L2_8, EV_L2_SUPER, l2_st8_got_super},
+	{ST_L2_7, EV_L2_I, l2_got_iframe},
+	{ST_L2_8, EV_L2_I, l2_got_iframe},
+	{ST_L2_5, EV_L2_T200, l2_st5_tout_200},
+	{ST_L2_6, EV_L2_T200, l2_st6_tout_200},
+	{ST_L2_7, EV_L2_T200, l2_st7_tout_200},
+	{ST_L2_8, EV_L2_T200, l2_st8_tout_200},
+	{ST_L2_7, EV_L2_T203, l2_st7_tout_203},
+	{ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue},
+	{ST_L2_7, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
+	{ST_L2_8, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
+	{ST_L2_7, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
+	{ST_L2_8, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
+	{ST_L2_4, EV_L2_FRAME_ERROR, l2_frame_error},
+	{ST_L2_5, EV_L2_FRAME_ERROR, l2_frame_error},
+	{ST_L2_6, EV_L2_FRAME_ERROR, l2_frame_error},
+	{ST_L2_7, EV_L2_FRAME_ERROR, l2_frame_error_reest},
+	{ST_L2_8, EV_L2_FRAME_ERROR, l2_frame_error_reest},
+	{ST_L2_1, EV_L1_DEACTIVATE, l2_st14_persistant_da},
+	{ST_L2_2, EV_L1_DEACTIVATE, l2_st24_tei_remove},
+	{ST_L2_3, EV_L1_DEACTIVATE, l2_st3_tei_remove},
+	{ST_L2_4, EV_L1_DEACTIVATE, l2_st14_persistant_da},
+	{ST_L2_5, EV_L1_DEACTIVATE, l2_st5_persistant_da},
+	{ST_L2_6, EV_L1_DEACTIVATE, l2_st6_persistant_da},
+	{ST_L2_7, EV_L1_DEACTIVATE, l2_persistant_da},
+	{ST_L2_8, EV_L1_DEACTIVATE, l2_persistant_da},
+};
+
+#define L2_FN_COUNT (sizeof(L2FnList)/sizeof(struct FsmNode))
+
+static int
+ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
+{
+	u_char	*datap = skb->data;
+	int	ret = -EINVAL;
+	int	psapi, ptei;
+	u_int	l;
+	int	c = 0;
+
+	l = l2addrsize(l2);
+	if (skb->len <= l) {
+		mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *) 'N');
+		return ret;
+	}
+	if (test_bit(FLG_LAPD, &l2->flag)) { /* Maybe not needed */
+		psapi = *datap++;
+		ptei = *datap++;
+		if ((psapi & 1) || !(ptei & 1)) {
+			printk(KERN_WARNING
+			    "l2 D-channel frame wrong EA0/EA1\n");
+			return ret;
+		}
+		psapi >>= 2;
+		ptei >>= 1;
+		if (psapi != l2->sapi) {
+			/* not our bussiness
+			 * printk(KERN_DEBUG "%s: sapi %d/%d sapi mismatch\n",
+			 *  __func__,
+			 *	psapi, l2->sapi);
+			 */
+			dev_kfree_skb(skb);
+			return 0;
+		}
+		if ((ptei != l2->tei) && (ptei != GROUP_TEI)) {
+			/* not our bussiness
+			 * printk(KERN_DEBUG "%s: tei %d/%d sapi %d mismatch\n",
+			 *  __func__,
+			 *	ptei, l2->tei, psapi);
+			 */
+			dev_kfree_skb(skb);
+			return 0;
+		}
+	} else
+		datap += l;
+	if (!(*datap & 1)) {	/* I-Frame */
+		c = iframe_error(l2, skb);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_I, skb);
+	} else if (IsSFrame(datap, l2)) {	/* S-Frame */
+		c = super_error(l2, skb);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SUPER, skb);
+	} else if (IsUI(datap)) {
+		c = UI_error(l2, skb);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UI, skb);
+	} else if (IsSABME(datap, l2)) {
+		c = unnum_error(l2, skb, CMD);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SABME, skb);
+	} else if (IsUA(datap)) {
+		c = unnum_error(l2, skb, RSP);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UA, skb);
+	} else if (IsDISC(datap)) {
+		c = unnum_error(l2, skb, CMD);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DISC, skb);
+	} else if (IsDM(datap)) {
+		c = unnum_error(l2, skb, RSP);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DM, skb);
+	} else if (IsFRMR(datap)) {
+		c = FRMR_error(l2, skb);
+		if (!c)
+			ret = mISDN_FsmEvent(&l2->l2m, EV_L2_FRMR, skb);
+	} else
+		c = 'L';
+	if (c) {
+		printk(KERN_WARNING "l2 D-channel frame error %c\n", c);
+		mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *)(long)c);
+	}
+	return ret;
+}
+
+static int
+l2_send(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct layer2		*l2 = container_of(ch, struct layer2, ch);
+	struct mISDNhead	*hh =  mISDN_HEAD_P(skb);
+	int 			ret = -EINVAL;
+
+	if (*debug & DEBUG_L2_RECV)
+		printk(KERN_DEBUG "%s: prim(%x) id(%x) tei(%d)\n",
+		    __func__, hh->prim, hh->id, l2->tei);
+	switch (hh->prim) {
+	case PH_DATA_IND:
+		ret = ph_data_indication(l2, hh, skb);
+		break;
+	case PH_DATA_CNF:
+		ret = ph_data_confirm(l2, hh, skb);
+		break;
+	case PH_ACTIVATE_IND:
+		test_and_set_bit(FLG_L1_ACTIV, &l2->flag);
+		l2up_create(l2, MPH_ACTIVATE_IND, 0, NULL);
+		if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag))
+			ret = mISDN_FsmEvent(&l2->l2m,
+				EV_L2_DL_ESTABLISH_REQ, skb);
+		break;
+	case PH_DEACTIVATE_IND:
+		test_and_clear_bit(FLG_L1_ACTIV, &l2->flag);
+		l2up_create(l2, MPH_DEACTIVATE_IND, 0, NULL);
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L1_DEACTIVATE, skb);
+		break;
+	case MPH_INFORMATION_IND:
+		if (!l2->up)
+			break;
+		ret = l2->up->send(l2->up, skb);
+		break;
+	case DL_DATA_REQ:
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_DATA, skb);
+		break;
+	case DL_UNITDATA_REQ:
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_UNITDATA, skb);
+		break;
+	case DL_ESTABLISH_REQ:
+		if (test_bit(FLG_LAPB, &l2->flag))
+			test_and_set_bit(FLG_ORIG, &l2->flag);
+		if (test_bit(FLG_L1_ACTIV, &l2->flag)) {
+			if (test_bit(FLG_LAPD, &l2->flag) ||
+				test_bit(FLG_ORIG, &l2->flag))
+				ret = mISDN_FsmEvent(&l2->l2m,
+					EV_L2_DL_ESTABLISH_REQ, skb);
+		} else {
+			if (test_bit(FLG_LAPD, &l2->flag) ||
+				test_bit(FLG_ORIG, &l2->flag)) {
+				test_and_set_bit(FLG_ESTAB_PEND,
+					&l2->flag);
+			}
+			ret = l2down(l2, PH_ACTIVATE_REQ, l2_newid(l2),
+			    skb);
+		}
+		break;
+	case DL_RELEASE_REQ:
+		if (test_bit(FLG_LAPB, &l2->flag))
+			l2down_create(l2, PH_DEACTIVATE_REQ,
+				l2_newid(l2), 0, NULL);
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_RELEASE_REQ,
+		    skb);
+		break;
+	default:
+		if (*debug & DEBUG_L2)
+			l2m_debug(&l2->l2m, "l2 unknown pr %04x",
+			    hh->prim);
+	}
+	if (ret) {
+		dev_kfree_skb(skb);
+		ret = 0;
+	}
+	return ret;
+}
+
+int
+tei_l2(struct layer2 *l2, u_int cmd, u_long arg)
+{
+	int		ret = -EINVAL;
+
+	if (*debug & DEBUG_L2_TEI)
+		printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd);
+	switch (cmd) {
+	case (MDL_ASSIGN_REQ):
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ASSIGN, (void *)arg);
+		break;
+	case (MDL_REMOVE_REQ):
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_REMOVE, NULL);
+		break;
+	case (MDL_ERROR_IND):
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL);
+		break;
+	case (MDL_ERROR_RSP):
+		/* ETS 300-125 5.3.2.1 Test: TC13010 */
+		printk(KERN_NOTICE "MDL_ERROR|REQ (tei_l2)\n");
+		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL);
+		break;
+	}
+	return ret;
+}
+
+static void
+release_l2(struct layer2 *l2)
+{
+	mISDN_FsmDelTimer(&l2->t200, 21);
+	mISDN_FsmDelTimer(&l2->t203, 16);
+	skb_queue_purge(&l2->i_queue);
+	skb_queue_purge(&l2->ui_queue);
+	skb_queue_purge(&l2->down_queue);
+	ReleaseWin(l2);
+	if (test_bit(FLG_LAPD, &l2->flag)) {
+		release_tei(l2);
+		if (l2->ch.st)
+			l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D,
+			    CLOSE_CHANNEL, NULL);
+	}
+	kfree(l2);
+}
+
+static int
+l2_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct layer2		*l2 = container_of(ch, struct layer2, ch);
+	u_int			info;
+
+	if (*debug & DEBUG_L2_CTRL)
+		printk(KERN_DEBUG "%s:(%x)\n", __func__, cmd);
+
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		if (test_bit(FLG_LAPD, &l2->flag)) {
+			set_channel_address(&l2->ch, l2->sapi, l2->tei);
+			info = DL_INFO_L2_CONNECT;
+			l2up_create(l2, DL_INFORMATION_IND,
+			    sizeof(info), &info);
+		}
+		break;
+	case CLOSE_CHANNEL:
+		if (l2->ch.peer)
+			l2->ch.peer->ctrl(l2->ch.peer, CLOSE_CHANNEL, NULL);
+		release_l2(l2);
+		break;
+	}
+	return 0;
+}
+
+struct layer2 *
+create_l2(struct mISDNchannel *ch, u_int protocol, u_long options, u_long arg)
+{
+	struct layer2		*l2;
+	struct channel_req	rq;
+
+	l2 = kzalloc(sizeof(struct layer2), GFP_KERNEL);
+	if (!l2) {
+		printk(KERN_ERR "kzalloc layer2 failed\n");
+		return NULL;
+	}
+	l2->next_id = 1;
+	l2->down_id = MISDN_ID_NONE;
+	l2->up = ch;
+	l2->ch.st = ch->st;
+	l2->ch.send = l2_send;
+	l2->ch.ctrl = l2_ctrl;
+	switch (protocol) {
+	case ISDN_P_LAPD_NT:
+		test_and_set_bit(FLG_LAPD, &l2->flag);
+		test_and_set_bit(FLG_LAPD_NET, &l2->flag);
+		test_and_set_bit(FLG_MOD128, &l2->flag);
+		l2->sapi = 0;
+		l2->maxlen = MAX_DFRAME_LEN;
+		if (test_bit(OPTION_L2_PMX, &options))
+			l2->window = 7;
+		else
+			l2->window = 1;
+		if (test_bit(OPTION_L2_PTP, &options))
+			test_and_set_bit(FLG_PTP, &l2->flag);
+		if (test_bit(OPTION_L2_FIXEDTEI, &options))
+			test_and_set_bit(FLG_FIXED_TEI, &l2->flag);
+		l2->tei = (u_int)arg;
+		l2->T200 = 1000;
+		l2->N200 = 3;
+		l2->T203 = 10000;
+		if (test_bit(OPTION_L2_PMX, &options))
+			rq.protocol = ISDN_P_NT_E1;
+		else
+			rq.protocol = ISDN_P_NT_S0;
+		rq.adr.channel = 0;
+		l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq);
+		break;
+	case ISDN_P_LAPD_TE:
+		test_and_set_bit(FLG_LAPD, &l2->flag);
+		test_and_set_bit(FLG_MOD128, &l2->flag);
+		test_and_set_bit(FLG_ORIG, &l2->flag);
+		l2->sapi = 0;
+		l2->maxlen = MAX_DFRAME_LEN;
+		if (test_bit(OPTION_L2_PMX, &options))
+			l2->window = 7;
+		else
+			l2->window = 1;
+		if (test_bit(OPTION_L2_PTP, &options))
+			test_and_set_bit(FLG_PTP, &l2->flag);
+		if (test_bit(OPTION_L2_FIXEDTEI, &options))
+			test_and_set_bit(FLG_FIXED_TEI, &l2->flag);
+		l2->tei = (u_int)arg;
+		l2->T200 = 1000;
+		l2->N200 = 3;
+		l2->T203 = 10000;
+		if (test_bit(OPTION_L2_PMX, &options))
+			rq.protocol = ISDN_P_TE_E1;
+		else
+			rq.protocol = ISDN_P_TE_S0;
+		rq.adr.channel = 0;
+		l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq);
+		break;
+	case ISDN_P_B_X75SLP:
+		test_and_set_bit(FLG_LAPB, &l2->flag);
+		l2->window = 7;
+		l2->maxlen = MAX_DATA_SIZE;
+		l2->T200 = 1000;
+		l2->N200 = 4;
+		l2->T203 = 5000;
+		l2->addr.A = 3;
+		l2->addr.B = 1;
+		break;
+	default:
+		printk(KERN_ERR "layer2 create failed prt %x\n",
+			protocol);
+		kfree(l2);
+		return NULL;
+	}
+	skb_queue_head_init(&l2->i_queue);
+	skb_queue_head_init(&l2->ui_queue);
+	skb_queue_head_init(&l2->down_queue);
+	skb_queue_head_init(&l2->tmp_queue);
+	InitWin(l2);
+	l2->l2m.fsm = &l2fsm;
+	if (test_bit(FLG_LAPB, &l2->flag) ||
+		test_bit(FLG_PTP, &l2->flag) ||
+		test_bit(FLG_LAPD_NET, &l2->flag))
+		l2->l2m.state = ST_L2_4;
+	else
+		l2->l2m.state = ST_L2_1;
+	l2->l2m.debug = *debug;
+	l2->l2m.userdata = l2;
+	l2->l2m.userint = 0;
+	l2->l2m.printdebug = l2m_debug;
+
+	mISDN_FsmInitTimer(&l2->l2m, &l2->t200);
+	mISDN_FsmInitTimer(&l2->l2m, &l2->t203);
+	return l2;
+}
+
+static int
+x75create(struct channel_req *crq)
+{
+	struct layer2	*l2;
+
+	if (crq->protocol != ISDN_P_B_X75SLP)
+		return -EPROTONOSUPPORT;
+	l2 = create_l2(crq->ch, crq->protocol, 0, 0);
+	if (!l2)
+		return -ENOMEM;
+	crq->ch = &l2->ch;
+	crq->protocol = ISDN_P_B_HDLC;
+	return 0;
+}
+
+static struct Bprotocol X75SLP = {
+	.Bprotocols = (1 << (ISDN_P_B_X75SLP & ISDN_P_B_MASK)),
+	.name = "X75SLP",
+	.create = x75create
+};
+
+int
+Isdnl2_Init(u_int *deb)
+{
+	debug = deb;
+	mISDN_register_Bprotocol(&X75SLP);
+	l2fsm.state_count = L2_STATE_COUNT;
+	l2fsm.event_count = L2_EVENT_COUNT;
+	l2fsm.strEvent = strL2Event;
+	l2fsm.strState = strL2State;
+	mISDN_FsmNew(&l2fsm, L2FnList, ARRAY_SIZE(L2FnList));
+	TEIInit(deb);
+	return 0;
+}
+
+void
+Isdnl2_cleanup(void)
+{
+	mISDN_unregister_Bprotocol(&X75SLP);
+	TEIFree();
+	mISDN_FsmFree(&l2fsm);
+}
+
diff --git a/drivers/isdn/mISDN/layer2.h b/drivers/isdn/mISDN/layer2.h
new file mode 100644
index 00000000000..de2dd02056a
--- /dev/null
+++ b/drivers/isdn/mISDN/layer2.h
@@ -0,0 +1,140 @@
+/*
+ * Layer 2 defines
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include <linux/skbuff.h>
+#include "fsm.h"
+
+#define MAX_WINDOW	8
+
+struct manager {
+	struct mISDNchannel	ch;
+	struct mISDNchannel	bcast;
+	u_long			options;
+	struct list_head	layer2;
+	rwlock_t		lock;
+	struct FsmInst		deact;
+	struct FsmTimer		datimer;
+	struct sk_buff_head	sendq;
+	struct mISDNchannel	*up;
+	u_int			nextid;
+	u_int			lastid;
+};
+
+struct teimgr {
+	int			ri;
+	int			rcnt;
+	struct FsmInst		tei_m;
+	struct FsmTimer		timer;
+	int			tval, nval;
+	struct layer2		*l2;
+	struct manager		*mgr;
+};
+
+struct laddr {
+	u_char	A;
+	u_char	B;
+};
+
+struct layer2 {
+	struct list_head	list;
+	struct mISDNchannel	ch;
+	u_long			flag;
+	int			id;
+	struct mISDNchannel	*up;
+	signed char		sapi;
+	signed char		tei;
+	struct laddr		addr;
+	u_int			maxlen;
+	struct teimgr		*tm;
+	u_int			vs, va, vr;
+	int			rc;
+	u_int			window;
+	u_int			sow;
+	struct FsmInst		l2m;
+	struct FsmTimer		t200, t203;
+	int			T200, N200, T203;
+	u_int			next_id;
+	u_int			down_id;
+	struct sk_buff		*windowar[MAX_WINDOW];
+	struct sk_buff_head	i_queue;
+	struct sk_buff_head	ui_queue;
+	struct sk_buff_head	down_queue;
+	struct sk_buff_head	tmp_queue;
+};
+
+enum {
+	ST_L2_1,
+	ST_L2_2,
+	ST_L2_3,
+	ST_L2_4,
+	ST_L2_5,
+	ST_L2_6,
+	ST_L2_7,
+	ST_L2_8,
+};
+
+#define L2_STATE_COUNT (ST_L2_8+1)
+
+extern struct layer2	*create_l2(struct mISDNchannel *, u_int,
+				u_long, u_long);
+extern int		tei_l2(struct layer2 *, u_int, u_long arg);
+
+
+/* from tei.c */
+extern int 		l2_tei(struct layer2 *, u_int, u_long arg);
+extern void 		release_tei(struct layer2 *);
+extern int 		TEIInit(u_int *);
+extern void 		TEIFree(void);
+
+#define MAX_L2HEADER_LEN 4
+
+#define RR	0x01
+#define RNR	0x05
+#define REJ	0x09
+#define SABME	0x6f
+#define SABM	0x2f
+#define DM	0x0f
+#define UI	0x03
+#define DISC	0x43
+#define UA	0x63
+#define FRMR	0x87
+#define XID	0xaf
+
+#define CMD	0
+#define RSP	1
+
+#define LC_FLUSH_WAIT 1
+
+#define FLG_LAPB	0
+#define FLG_LAPD	1
+#define FLG_ORIG	2
+#define FLG_MOD128	3
+#define FLG_PEND_REL	4
+#define FLG_L3_INIT	5
+#define FLG_T200_RUN	6
+#define FLG_ACK_PEND	7
+#define FLG_REJEXC	8
+#define FLG_OWN_BUSY	9
+#define FLG_PEER_BUSY	10
+#define FLG_DCHAN_BUSY	11
+#define FLG_L1_ACTIV	12
+#define FLG_ESTAB_PEND	13
+#define FLG_PTP		14
+#define FLG_FIXED_TEI	15
+#define FLG_L2BLOCK	16
+#define FLG_L1_NOTREADY	17
+#define FLG_LAPD_NET	18
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
new file mode 100644
index 00000000000..4ba4cc364c9
--- /dev/null
+++ b/drivers/isdn/mISDN/socket.c
@@ -0,0 +1,781 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include "core.h"
+
+static int	*debug;
+
+static struct proto mISDN_proto = {
+	.name		= "misdn",
+	.owner		= THIS_MODULE,
+	.obj_size	= sizeof(struct mISDN_sock)
+};
+
+#define _pms(sk)	((struct mISDN_sock *)sk)
+
+static struct mISDN_sock_list	data_sockets = {
+	.lock = __RW_LOCK_UNLOCKED(data_sockets.lock)
+};
+
+static struct mISDN_sock_list	base_sockets = {
+	.lock = __RW_LOCK_UNLOCKED(base_sockets.lock)
+};
+
+#define L2_HEADER_LEN	4
+
+static inline struct sk_buff *
+_l2_alloc_skb(unsigned int len, gfp_t gfp_mask)
+{
+	struct sk_buff  *skb;
+
+	skb = alloc_skb(len + L2_HEADER_LEN, gfp_mask);
+	if (likely(skb))
+		skb_reserve(skb, L2_HEADER_LEN);
+	return skb;
+}
+
+static void
+mISDN_sock_link(struct mISDN_sock_list *l, struct sock *sk)
+{
+	write_lock_bh(&l->lock);
+	sk_add_node(sk, &l->head);
+	write_unlock_bh(&l->lock);
+}
+
+static void mISDN_sock_unlink(struct mISDN_sock_list *l, struct sock *sk)
+{
+	write_lock_bh(&l->lock);
+	sk_del_node_init(sk);
+	write_unlock_bh(&l->lock);
+}
+
+static int
+mISDN_send(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct mISDN_sock *msk;
+	int	err;
+
+	msk = container_of(ch, struct mISDN_sock, ch);
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s len %d %p\n", __func__, skb->len, skb);
+	if (msk->sk.sk_state == MISDN_CLOSED)
+		return -EUNATCH;
+	__net_timestamp(skb);
+	err = sock_queue_rcv_skb(&msk->sk, skb);
+	if (err)
+		printk(KERN_WARNING "%s: error %d\n", __func__, err);
+	return err;
+}
+
+static int
+mISDN_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct mISDN_sock *msk;
+
+	msk = container_of(ch, struct mISDN_sock, ch);
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s(%p, %x, %p)\n", __func__, ch, cmd, arg);
+	switch (cmd) {
+	case CLOSE_CHANNEL:
+		msk->sk.sk_state = MISDN_CLOSED;
+		break;
+	}
+	return 0;
+}
+
+static inline void
+mISDN_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_buff *skb)
+{
+	struct timeval	tv;
+
+	if (_pms(sk)->cmask & MISDN_TIME_STAMP) {
+		skb_get_timestamp(skb, &tv);
+		put_cmsg(msg, SOL_MISDN, MISDN_TIME_STAMP, sizeof(tv), &tv);
+	}
+}
+
+static int
+mISDN_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
+    struct msghdr *msg, size_t len, int flags)
+{
+	struct sk_buff		*skb;
+	struct sock		*sk = sock->sk;
+	struct sockaddr_mISDN	*maddr;
+
+	int		copied, err;
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s: len %d, flags %x ch.nr %d, proto %x\n",
+			__func__, (int)len, flags, _pms(sk)->ch.nr,
+			sk->sk_protocol);
+	if (flags & (MSG_OOB))
+		return -EOPNOTSUPP;
+
+	if (sk->sk_state == MISDN_CLOSED)
+		return 0;
+
+	skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
+	if (!skb)
+		return err;
+
+	if (msg->msg_namelen >= sizeof(struct sockaddr_mISDN)) {
+		msg->msg_namelen = sizeof(struct sockaddr_mISDN);
+		maddr = (struct sockaddr_mISDN *)msg->msg_name;
+		maddr->family = AF_ISDN;
+		maddr->dev = _pms(sk)->dev->id;
+		if ((sk->sk_protocol == ISDN_P_LAPD_TE) ||
+		    (sk->sk_protocol == ISDN_P_LAPD_NT)) {
+			maddr->channel = (mISDN_HEAD_ID(skb) >> 16) & 0xff;
+			maddr->tei =  (mISDN_HEAD_ID(skb) >> 8) & 0xff;
+			maddr->sapi = mISDN_HEAD_ID(skb) & 0xff;
+		} else {
+			maddr->channel = _pms(sk)->ch.nr;
+			maddr->sapi = _pms(sk)->ch.addr & 0xFF;
+			maddr->tei =  (_pms(sk)->ch.addr >> 8) & 0xFF;
+		}
+	} else {
+		if (msg->msg_namelen)
+			printk(KERN_WARNING "%s: too small namelen %d\n",
+			    __func__, msg->msg_namelen);
+		msg->msg_namelen = 0;
+	}
+
+	copied = skb->len + MISDN_HEADER_LEN;
+	if (len < copied) {
+		if (flags & MSG_PEEK)
+			atomic_dec(&skb->users);
+		else
+			skb_queue_head(&sk->sk_receive_queue, skb);
+		return -ENOSPC;
+	}
+	memcpy(skb_push(skb, MISDN_HEADER_LEN), mISDN_HEAD_P(skb),
+	    MISDN_HEADER_LEN);
+
+	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
+
+	mISDN_sock_cmsg(sk, msg, skb);
+
+	skb_free_datagram(sk, skb);
+
+	return err ? : copied;
+}
+
+static int
+mISDN_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
+    struct msghdr *msg, size_t len)
+{
+	struct sock		*sk = sock->sk;
+	struct sk_buff		*skb;
+	int			err = -ENOMEM;
+	struct sockaddr_mISDN	*maddr;
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s: len %d flags %x ch %d proto %x\n",
+		     __func__, (int)len, msg->msg_flags, _pms(sk)->ch.nr,
+		     sk->sk_protocol);
+
+	if (msg->msg_flags & MSG_OOB)
+		return -EOPNOTSUPP;
+
+	if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE))
+		return -EINVAL;
+
+	if (len < MISDN_HEADER_LEN)
+		return -EINVAL;
+
+	if (sk->sk_state != MISDN_BOUND)
+		return -EBADFD;
+
+	lock_sock(sk);
+
+	skb = _l2_alloc_skb(len, GFP_KERNEL);
+	if (!skb)
+		goto done;
+
+	if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
+		err = -EFAULT;
+		goto drop;
+	}
+
+	memcpy(mISDN_HEAD_P(skb), skb->data, MISDN_HEADER_LEN);
+	skb_pull(skb, MISDN_HEADER_LEN);
+
+	if (msg->msg_namelen >= sizeof(struct sockaddr_mISDN)) {
+		/* if we have a address, we use it */
+		maddr = (struct sockaddr_mISDN *)msg->msg_name;
+		mISDN_HEAD_ID(skb) = maddr->channel;
+	} else { /* use default for L2 messages */
+		if ((sk->sk_protocol == ISDN_P_LAPD_TE) ||
+		    (sk->sk_protocol == ISDN_P_LAPD_NT))
+		    mISDN_HEAD_ID(skb) = _pms(sk)->ch.nr;
+	}
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s: ID:%x\n",
+		     __func__, mISDN_HEAD_ID(skb));
+
+	err = -ENODEV;
+	if (!_pms(sk)->ch.peer ||
+	    (err = _pms(sk)->ch.recv(_pms(sk)->ch.peer, skb)))
+		goto drop;
+
+	err = len;
+
+done:
+	release_sock(sk);
+	return err;
+
+drop:
+	kfree_skb(skb);
+	goto done;
+}
+
+static int
+data_sock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk);
+	if (!sk)
+		return 0;
+	switch (sk->sk_protocol) {
+	case ISDN_P_TE_S0:
+	case ISDN_P_NT_S0:
+	case ISDN_P_TE_E1:
+	case ISDN_P_NT_E1:
+		if (sk->sk_state == MISDN_BOUND)
+			delete_channel(&_pms(sk)->ch);
+		else
+			mISDN_sock_unlink(&data_sockets, sk);
+		break;
+	case ISDN_P_LAPD_TE:
+	case ISDN_P_LAPD_NT:
+	case ISDN_P_B_RAW:
+	case ISDN_P_B_HDLC:
+	case ISDN_P_B_X75SLP:
+	case ISDN_P_B_L2DTMF:
+	case ISDN_P_B_L2DSP:
+	case ISDN_P_B_L2DSPHDLC:
+		delete_channel(&_pms(sk)->ch);
+		mISDN_sock_unlink(&data_sockets, sk);
+		break;
+	}
+
+	lock_sock(sk);
+
+	sock_orphan(sk);
+	skb_queue_purge(&sk->sk_receive_queue);
+
+	release_sock(sk);
+	sock_put(sk);
+
+	return 0;
+}
+
+static int
+data_sock_ioctl_bound(struct sock *sk, unsigned int cmd, void __user *p)
+{
+	struct mISDN_ctrl_req	cq;
+	int			err = -EINVAL, val;
+	struct mISDNchannel	*bchan, *next;
+
+	lock_sock(sk);
+	if (!_pms(sk)->dev) {
+		err = -ENODEV;
+		goto done;
+	}
+	switch (cmd) {
+	case IMCTRLREQ:
+		if (copy_from_user(&cq, p, sizeof(cq))) {
+			err = -EFAULT;
+			break;
+		}
+		if ((sk->sk_protocol & ~ISDN_P_B_MASK) == ISDN_P_B_START) {
+			list_for_each_entry_safe(bchan, next,
+				&_pms(sk)->dev->bchannels, list) {
+				if (bchan->nr == cq.channel) {
+					err = bchan->ctrl(bchan,
+						CONTROL_CHANNEL, &cq);
+					break;
+				}
+			}
+		} else
+			err = _pms(sk)->dev->D.ctrl(&_pms(sk)->dev->D,
+				CONTROL_CHANNEL, &cq);
+		if (err)
+			break;
+		if (copy_to_user(p, &cq, sizeof(cq)))
+			err = -EFAULT;
+		break;
+	case IMCLEAR_L2:
+		if (sk->sk_protocol != ISDN_P_LAPD_NT) {
+			err = -EINVAL;
+			break;
+		}
+		if (get_user(val, (int __user *)p)) {
+			err = -EFAULT;
+			break;
+		}
+		err = _pms(sk)->dev->teimgr->ctrl(_pms(sk)->dev->teimgr,
+		    CONTROL_CHANNEL, &val);
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+done:
+	release_sock(sk);
+	return err;
+}
+
+static int
+data_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
+{
+	int 			err = 0, id;
+	struct sock		*sk = sock->sk;
+	struct mISDNdevice	*dev;
+	struct mISDNversion	ver;
+
+	switch (cmd) {
+	case IMGETVERSION:
+		ver.major = MISDN_MAJOR_VERSION;
+		ver.minor = MISDN_MINOR_VERSION;
+		ver.release = MISDN_RELEASE;
+		if (copy_to_user((void __user *)arg, &ver, sizeof(ver)))
+			err = -EFAULT;
+		break;
+	case IMGETCOUNT:
+		id = get_mdevice_count();
+		if (put_user(id, (int __user *)arg))
+			err = -EFAULT;
+		break;
+	case IMGETDEVINFO:
+		if (get_user(id, (int __user *)arg)) {
+			err = -EFAULT;
+			break;
+		}
+		dev = get_mdevice(id);
+		if (dev) {
+			struct mISDN_devinfo di;
+
+			di.id = dev->id;
+			di.Dprotocols = dev->Dprotocols;
+			di.Bprotocols = dev->Bprotocols | get_all_Bprotocols();
+			di.protocol = dev->D.protocol;
+			memcpy(di.channelmap, dev->channelmap,
+				MISDN_CHMAP_SIZE * 4);
+			di.nrbchan = dev->nrbchan;
+			strcpy(di.name, dev->name);
+			if (copy_to_user((void __user *)arg, &di, sizeof(di)))
+				err = -EFAULT;
+		} else
+			err = -ENODEV;
+		break;
+	default:
+		if (sk->sk_state == MISDN_BOUND)
+			err = data_sock_ioctl_bound(sk, cmd,
+				(void __user *)arg);
+		else
+			err = -ENOTCONN;
+	}
+	return err;
+}
+
+static int data_sock_setsockopt(struct socket *sock, int level, int optname,
+	char __user *optval, int len)
+{
+	struct sock *sk = sock->sk;
+	int err = 0, opt = 0;
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s(%p, %d, %x, %p, %d)\n", __func__, sock,
+		    level, optname, optval, len);
+
+	lock_sock(sk);
+
+	switch (optname) {
+	case MISDN_TIME_STAMP:
+		if (get_user(opt, (int __user *)optval)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (opt)
+			_pms(sk)->cmask |= MISDN_TIME_STAMP;
+		else
+			_pms(sk)->cmask &= ~MISDN_TIME_STAMP;
+		break;
+	default:
+		err = -ENOPROTOOPT;
+		break;
+	}
+	release_sock(sk);
+	return err;
+}
+
+static int data_sock_getsockopt(struct socket *sock, int level, int optname,
+	char __user *optval, int __user *optlen)
+{
+	struct sock *sk = sock->sk;
+	int len, opt;
+
+	if (get_user(len, optlen))
+		return -EFAULT;
+
+	switch (optname) {
+	case MISDN_TIME_STAMP:
+		if (_pms(sk)->cmask & MISDN_TIME_STAMP)
+			opt = 1;
+		else
+			opt = 0;
+
+		if (put_user(opt, optval))
+			return -EFAULT;
+		break;
+	default:
+		return -ENOPROTOOPT;
+	}
+
+	return 0;
+}
+
+static int
+data_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
+{
+	struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr;
+	struct sock *sk = sock->sk;
+	int err = 0;
+
+	if (*debug & DEBUG_SOCKET)
+		printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk);
+	if (addr_len != sizeof(struct sockaddr_mISDN))
+		return -EINVAL;
+	if (!maddr || maddr->family != AF_ISDN)
+		return -EINVAL;
+
+	lock_sock(sk);
+
+	if (_pms(sk)->dev) {
+		err = -EALREADY;
+		goto done;
+	}
+	_pms(sk)->dev = get_mdevice(maddr->dev);
+	if (!_pms(sk)->dev) {
+		err = -ENODEV;
+		goto done;
+	}
+	_pms(sk)->ch.send = mISDN_send;
+	_pms(sk)->ch.ctrl = mISDN_ctrl;
+
+	switch (sk->sk_protocol) {
+	case ISDN_P_TE_S0:
+	case ISDN_P_NT_S0:
+	case ISDN_P_TE_E1:
+	case ISDN_P_NT_E1:
+		mISDN_sock_unlink(&data_sockets, sk);
+		err = connect_layer1(_pms(sk)->dev, &_pms(sk)->ch,
+		    sk->sk_protocol, maddr);
+		if (err)
+			mISDN_sock_link(&data_sockets, sk);
+		break;
+	case ISDN_P_LAPD_TE:
+	case ISDN_P_LAPD_NT:
+		err = create_l2entity(_pms(sk)->dev, &_pms(sk)->ch,
+		    sk->sk_protocol, maddr);
+		break;
+	case ISDN_P_B_RAW:
+	case ISDN_P_B_HDLC:
+	case ISDN_P_B_X75SLP:
+	case ISDN_P_B_L2DTMF:
+	case ISDN_P_B_L2DSP:
+	case ISDN_P_B_L2DSPHDLC:
+		err = connect_Bstack(_pms(sk)->dev, &_pms(sk)->ch,
+		    sk->sk_protocol, maddr);
+		break;
+	default:
+		err = -EPROTONOSUPPORT;
+	}
+	if (err)
+		goto done;
+	sk->sk_state = MISDN_BOUND;
+	_pms(sk)->ch.protocol = sk->sk_protocol;
+
+done:
+	release_sock(sk);
+	return err;
+}
+
+static int
+data_sock_getname(struct socket *sock, struct sockaddr *addr,
+    int *addr_len, int peer)
+{
+	struct sockaddr_mISDN 	*maddr = (struct sockaddr_mISDN *) addr;
+	struct sock		*sk = sock->sk;
+
+	if (!_pms(sk)->dev)
+		return -EBADFD;
+
+	lock_sock(sk);
+
+	*addr_len = sizeof(*maddr);
+	maddr->dev = _pms(sk)->dev->id;
+	maddr->channel = _pms(sk)->ch.nr;
+	maddr->sapi = _pms(sk)->ch.addr & 0xff;
+	maddr->tei = (_pms(sk)->ch.addr >> 8) & 0xff;
+	release_sock(sk);
+	return 0;
+}
+
+static const struct proto_ops data_sock_ops = {
+	.family		= PF_ISDN,
+	.owner		= THIS_MODULE,
+	.release	= data_sock_release,
+	.ioctl		= data_sock_ioctl,
+	.bind		= data_sock_bind,
+	.getname	= data_sock_getname,
+	.sendmsg	= mISDN_sock_sendmsg,
+	.recvmsg	= mISDN_sock_recvmsg,
+	.poll		= datagram_poll,
+	.listen		= sock_no_listen,
+	.shutdown	= sock_no_shutdown,
+	.setsockopt	= data_sock_setsockopt,
+	.getsockopt	= data_sock_getsockopt,
+	.connect	= sock_no_connect,
+	.socketpair	= sock_no_socketpair,
+	.accept		= sock_no_accept,
+	.mmap		= sock_no_mmap
+};
+
+static int
+data_sock_create(struct net *net, struct socket *sock, int protocol)
+{
+	struct sock *sk;
+
+	if (sock->type != SOCK_DGRAM)
+		return -ESOCKTNOSUPPORT;
+
+	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+
+	sock->ops = &data_sock_ops;
+	sock->state = SS_UNCONNECTED;
+	sock_reset_flag(sk, SOCK_ZAPPED);
+
+	sk->sk_protocol = protocol;
+	sk->sk_state    = MISDN_OPEN;
+	mISDN_sock_link(&data_sockets, sk);
+
+	return 0;
+}
+
+static int
+base_sock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk);
+	if (!sk)
+		return 0;
+
+	mISDN_sock_unlink(&base_sockets, sk);
+	sock_orphan(sk);
+	sock_put(sk);
+
+	return 0;
+}
+
+static int
+base_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
+{
+	int 			err = 0, id;
+	struct mISDNdevice	*dev;
+	struct mISDNversion	ver;
+
+	switch (cmd) {
+	case IMGETVERSION:
+		ver.major = MISDN_MAJOR_VERSION;
+		ver.minor = MISDN_MINOR_VERSION;
+		ver.release = MISDN_RELEASE;
+		if (copy_to_user((void __user *)arg, &ver, sizeof(ver)))
+			err = -EFAULT;
+		break;
+	case IMGETCOUNT:
+		id = get_mdevice_count();
+		if (put_user(id, (int __user *)arg))
+			err = -EFAULT;
+		break;
+	case IMGETDEVINFO:
+		if (get_user(id, (int __user *)arg)) {
+			err = -EFAULT;
+			break;
+		}
+		dev = get_mdevice(id);
+		if (dev) {
+			struct mISDN_devinfo di;
+
+			di.id = dev->id;
+			di.Dprotocols = dev->Dprotocols;
+			di.Bprotocols = dev->Bprotocols | get_all_Bprotocols();
+			di.protocol = dev->D.protocol;
+			memcpy(di.channelmap, dev->channelmap,
+				MISDN_CHMAP_SIZE * 4);
+			di.nrbchan = dev->nrbchan;
+			strcpy(di.name, dev->name);
+			if (copy_to_user((void __user *)arg, &di, sizeof(di)))
+				err = -EFAULT;
+		} else
+			err = -ENODEV;
+		break;
+	default:
+		err = -EINVAL;
+	}
+	return err;
+}
+
+static int
+base_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
+{
+	struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr;
+	struct sock *sk = sock->sk;
+	int err = 0;
+
+	if (!maddr || maddr->family != AF_ISDN)
+		return -EINVAL;
+
+	lock_sock(sk);
+
+	if (_pms(sk)->dev) {
+		err = -EALREADY;
+		goto done;
+	}
+
+	_pms(sk)->dev = get_mdevice(maddr->dev);
+	if (!_pms(sk)->dev) {
+		err = -ENODEV;
+		goto done;
+	}
+	sk->sk_state = MISDN_BOUND;
+
+done:
+	release_sock(sk);
+	return err;
+}
+
+static const struct proto_ops base_sock_ops = {
+	.family		= PF_ISDN,
+	.owner		= THIS_MODULE,
+	.release	= base_sock_release,
+	.ioctl		= base_sock_ioctl,
+	.bind		= base_sock_bind,
+	.getname	= sock_no_getname,
+	.sendmsg	= sock_no_sendmsg,
+	.recvmsg	= sock_no_recvmsg,
+	.poll		= sock_no_poll,
+	.listen		= sock_no_listen,
+	.shutdown	= sock_no_shutdown,
+	.setsockopt	= sock_no_setsockopt,
+	.getsockopt	= sock_no_getsockopt,
+	.connect	= sock_no_connect,
+	.socketpair	= sock_no_socketpair,
+	.accept		= sock_no_accept,
+	.mmap		= sock_no_mmap
+};
+
+
+static int
+base_sock_create(struct net *net, struct socket *sock, int protocol)
+{
+	struct sock *sk;
+
+	if (sock->type != SOCK_RAW)
+		return -ESOCKTNOSUPPORT;
+
+	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+	sock->ops = &base_sock_ops;
+	sock->state = SS_UNCONNECTED;
+	sock_reset_flag(sk, SOCK_ZAPPED);
+	sk->sk_protocol = protocol;
+	sk->sk_state    = MISDN_OPEN;
+	mISDN_sock_link(&base_sockets, sk);
+
+	return 0;
+}
+
+static int
+mISDN_sock_create(struct net *net, struct socket *sock, int proto)
+{
+	int err = -EPROTONOSUPPORT;
+
+	switch	(proto) {
+	case ISDN_P_BASE:
+		err = base_sock_create(net, sock, proto);
+		break;
+	case ISDN_P_TE_S0:
+	case ISDN_P_NT_S0:
+	case ISDN_P_TE_E1:
+	case ISDN_P_NT_E1:
+	case ISDN_P_LAPD_TE:
+	case ISDN_P_LAPD_NT:
+	case ISDN_P_B_RAW:
+	case ISDN_P_B_HDLC:
+	case ISDN_P_B_X75SLP:
+	case ISDN_P_B_L2DTMF:
+	case ISDN_P_B_L2DSP:
+	case ISDN_P_B_L2DSPHDLC:
+		err = data_sock_create(net, sock, proto);
+		break;
+	default:
+		return err;
+	}
+
+	return err;
+}
+
+static struct
+net_proto_family mISDN_sock_family_ops = {
+	.owner  = THIS_MODULE,
+	.family = PF_ISDN,
+	.create = mISDN_sock_create,
+};
+
+int
+misdn_sock_init(u_int *deb)
+{
+	int err;
+
+	debug = deb;
+	err = sock_register(&mISDN_sock_family_ops);
+	if (err)
+		printk(KERN_ERR "%s: error(%d)\n", __func__, err);
+	return err;
+}
+
+void
+misdn_sock_cleanup(void)
+{
+	sock_unregister(PF_ISDN);
+}
+
diff --git a/drivers/isdn/mISDN/stack.c b/drivers/isdn/mISDN/stack.c
new file mode 100644
index 00000000000..54cfddcc478
--- /dev/null
+++ b/drivers/isdn/mISDN/stack.c
@@ -0,0 +1,674 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/mISDNif.h>
+#include <linux/kthread.h>
+#include "core.h"
+
+static u_int	*debug;
+
+static inline void
+_queue_message(struct mISDNstack *st, struct sk_buff *skb)
+{
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+
+	if (*debug & DEBUG_QUEUE_FUNC)
+		printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
+		    __func__, hh->prim, hh->id, skb);
+	skb_queue_tail(&st->msgq, skb);
+	if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
+		test_and_set_bit(mISDN_STACK_WORK, &st->status);
+		wake_up_interruptible(&st->workq);
+	}
+}
+
+int
+mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	_queue_message(ch->st, skb);
+	return 0;
+}
+
+static struct mISDNchannel *
+get_channel4id(struct mISDNstack *st, u_int id)
+{
+	struct mISDNchannel	*ch;
+
+	mutex_lock(&st->lmutex);
+	list_for_each_entry(ch, &st->layer2, list) {
+		if (id == ch->nr)
+			goto unlock;
+	}
+	ch = NULL;
+unlock:
+	mutex_unlock(&st->lmutex);
+	return ch;
+}
+
+static void
+send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
+{
+	struct hlist_node	*node;
+	struct sock		*sk;
+	struct sk_buff		*cskb = NULL;
+
+	read_lock(&sl->lock);
+	sk_for_each(sk, node, &sl->head) {
+		if (sk->sk_state != MISDN_BOUND)
+			continue;
+		if (!cskb)
+			cskb = skb_copy(skb, GFP_KERNEL);
+		if (!cskb) {
+			printk(KERN_WARNING "%s no skb\n", __func__);
+			break;
+		}
+		if (!sock_queue_rcv_skb(sk, cskb))
+			cskb = NULL;
+	}
+	read_unlock(&sl->lock);
+	if (cskb)
+		dev_kfree_skb(cskb);
+}
+
+static void
+send_layer2(struct mISDNstack *st, struct sk_buff *skb)
+{
+	struct sk_buff		*cskb;
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	struct mISDNchannel	*ch;
+	int			ret;
+
+	if (!st)
+		return;
+	mutex_lock(&st->lmutex);
+	if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */
+		list_for_each_entry(ch, &st->layer2, list) {
+			if (list_is_last(&ch->list, &st->layer2)) {
+				cskb = skb;
+				skb = NULL;
+			} else {
+				cskb = skb_copy(skb, GFP_KERNEL);
+			}
+			if (cskb) {
+				ret = ch->send(ch, cskb);
+				if (ret) {
+					if (*debug & DEBUG_SEND_ERR)
+						printk(KERN_DEBUG
+						    "%s ch%d prim(%x) addr(%x)"
+						    " err %d\n",
+						    __func__, ch->nr,
+						    hh->prim, ch->addr, ret);
+					dev_kfree_skb(cskb);
+				}
+			} else {
+				printk(KERN_WARNING "%s ch%d addr %x no mem\n",
+				    __func__, ch->nr, ch->addr);
+				goto out;
+			}
+		}
+	} else {
+		list_for_each_entry(ch, &st->layer2, list) {
+			if ((hh->id & MISDN_ID_ADDR_MASK) == ch->addr) {
+				ret = ch->send(ch, skb);
+				if (!ret)
+					skb = NULL;
+				goto out;
+			}
+		}
+		ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
+		if (!ret)
+			skb = NULL;
+		else if (*debug & DEBUG_SEND_ERR)
+			printk(KERN_DEBUG
+			    "%s ch%d mgr prim(%x) addr(%x) err %d\n",
+			    __func__, ch->nr, hh->prim, ch->addr, ret);
+	}
+out:
+	mutex_unlock(&st->lmutex);
+	if (skb)
+		dev_kfree_skb(skb);
+}
+
+static inline int
+send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
+{
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	struct mISDNchannel	*ch;
+	int	lm;
+
+	lm = hh->prim & MISDN_LAYERMASK;
+	if (*debug & DEBUG_QUEUE_FUNC)
+		printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
+		    __func__, hh->prim, hh->id, skb);
+	if (lm == 0x1) {
+		if (!hlist_empty(&st->l1sock.head)) {
+			__net_timestamp(skb);
+			send_socklist(&st->l1sock, skb);
+		}
+		return st->layer1->send(st->layer1, skb);
+	} else if (lm == 0x2) {
+		if (!hlist_empty(&st->l1sock.head))
+			send_socklist(&st->l1sock, skb);
+		send_layer2(st, skb);
+		return 0;
+	} else if (lm == 0x4) {
+		ch = get_channel4id(st, hh->id);
+		if (ch)
+			return ch->send(ch, skb);
+		else
+			printk(KERN_WARNING
+			    "%s: dev(%s) prim(%x) id(%x) no channel\n",
+			    __func__, st->dev->name, hh->prim, hh->id);
+	} else if (lm == 0x8) {
+		WARN_ON(lm == 0x8);
+		ch = get_channel4id(st, hh->id);
+		if (ch)
+			return ch->send(ch, skb);
+		else
+			printk(KERN_WARNING
+			    "%s: dev(%s) prim(%x) id(%x) no channel\n",
+			    __func__, st->dev->name, hh->prim, hh->id);
+	} else {
+		/* broadcast not handled yet */
+		printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
+		    __func__, st->dev->name, hh->prim);
+	}
+	return -ESRCH;
+}
+
+static void
+do_clear_stack(struct mISDNstack *st)
+{
+}
+
+static int
+mISDNStackd(void *data)
+{
+	struct mISDNstack *st = data;
+	int err = 0;
+
+#ifdef CONFIG_SMP
+	lock_kernel();
+#endif
+	sigfillset(&current->blocked);
+#ifdef CONFIG_SMP
+	unlock_kernel();
+#endif
+	if (*debug & DEBUG_MSG_THREAD)
+		printk(KERN_DEBUG "mISDNStackd %s started\n", st->dev->name);
+
+	if (st->notify != NULL) {
+		complete(st->notify);
+		st->notify = NULL;
+	}
+
+	for (;;) {
+		struct sk_buff	*skb;
+
+		if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
+			test_and_clear_bit(mISDN_STACK_WORK, &st->status);
+			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
+		} else
+			test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
+		while (test_bit(mISDN_STACK_WORK, &st->status)) {
+			skb = skb_dequeue(&st->msgq);
+			if (!skb) {
+				test_and_clear_bit(mISDN_STACK_WORK,
+					&st->status);
+				/* test if a race happens */
+				skb = skb_dequeue(&st->msgq);
+				if (!skb)
+					continue;
+				test_and_set_bit(mISDN_STACK_WORK,
+				    &st->status);
+			}
+#ifdef MISDN_MSG_STATS
+			st->msg_cnt++;
+#endif
+			err = send_msg_to_layer(st, skb);
+			if (unlikely(err)) {
+				if (*debug & DEBUG_SEND_ERR)
+					printk(KERN_DEBUG
+					    "%s: %s prim(%x) id(%x) "
+					    "send call(%d)\n",
+					    __func__, st->dev->name,
+					    mISDN_HEAD_PRIM(skb),
+					    mISDN_HEAD_ID(skb), err);
+				dev_kfree_skb(skb);
+				continue;
+			}
+			if (unlikely(test_bit(mISDN_STACK_STOPPED,
+			    &st->status))) {
+				test_and_clear_bit(mISDN_STACK_WORK,
+				    &st->status);
+				test_and_clear_bit(mISDN_STACK_RUNNING,
+				    &st->status);
+				break;
+			}
+		}
+		if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
+			test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
+			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
+			do_clear_stack(st);
+			test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
+			test_and_set_bit(mISDN_STACK_RESTART, &st->status);
+		}
+		if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
+			test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
+			test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
+			if (!skb_queue_empty(&st->msgq))
+				test_and_set_bit(mISDN_STACK_WORK,
+				    &st->status);
+		}
+		if (test_bit(mISDN_STACK_ABORT, &st->status))
+			break;
+		if (st->notify != NULL) {
+			complete(st->notify);
+			st->notify = NULL;
+		}
+#ifdef MISDN_MSG_STATS
+		st->sleep_cnt++;
+#endif
+		test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
+		wait_event_interruptible(st->workq, (st->status &
+		    mISDN_STACK_ACTION_MASK));
+		if (*debug & DEBUG_MSG_THREAD)
+			printk(KERN_DEBUG "%s: %s wake status %08lx\n",
+			    __func__, st->dev->name, st->status);
+		test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
+
+		test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
+
+		if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
+			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
+#ifdef MISDN_MSG_STATS
+			st->stopped_cnt++;
+#endif
+		}
+	}
+#ifdef MISDN_MSG_STATS
+	printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
+	    "msg %d sleep %d stopped\n",
+	    st->dev->name, st->msg_cnt, st->sleep_cnt, st->stopped_cnt);
+	printk(KERN_DEBUG
+	    "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
+	    st->dev->name, st->thread->utime, st->thread->stime);
+	printk(KERN_DEBUG
+	    "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
+	    st->dev->name, st->thread->nvcsw, st->thread->nivcsw);
+	printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
+	    st->dev->name);
+#endif
+	test_and_set_bit(mISDN_STACK_KILLED, &st->status);
+	test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
+	test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
+	test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
+	skb_queue_purge(&st->msgq);
+	st->thread = NULL;
+	if (st->notify != NULL) {
+		complete(st->notify);
+		st->notify = NULL;
+	}
+	return 0;
+}
+
+static int
+l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	if (!ch->st)
+		return -ENODEV;
+	__net_timestamp(skb);
+	_queue_message(ch->st, skb);
+	return 0;
+}
+
+void
+set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
+{
+	ch->addr = sapi | (tei << 8);
+}
+
+void
+__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
+{
+	list_add_tail(&ch->list, &st->layer2);
+}
+
+void
+add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
+{
+	mutex_lock(&st->lmutex);
+	__add_layer2(ch, st);
+	mutex_unlock(&st->lmutex);
+}
+
+static int
+st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	if (!ch->st || ch->st->layer1)
+		return -EINVAL;
+	return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
+}
+
+int
+create_stack(struct mISDNdevice *dev)
+{
+	struct mISDNstack	*newst;
+	int			err;
+	DECLARE_COMPLETION_ONSTACK(done);
+
+	newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
+	if (!newst) {
+		printk(KERN_ERR "kmalloc mISDN_stack failed\n");
+		return -ENOMEM;
+	}
+	newst->dev = dev;
+	INIT_LIST_HEAD(&newst->layer2);
+	INIT_HLIST_HEAD(&newst->l1sock.head);
+	rwlock_init(&newst->l1sock.lock);
+	init_waitqueue_head(&newst->workq);
+	skb_queue_head_init(&newst->msgq);
+	mutex_init(&newst->lmutex);
+	dev->D.st = newst;
+	err = create_teimanager(dev);
+	if (err) {
+		printk(KERN_ERR "kmalloc teimanager failed\n");
+		kfree(newst);
+		return err;
+	}
+	dev->teimgr->peer = &newst->own;
+	dev->teimgr->recv = mISDN_queue_message;
+	dev->teimgr->st = newst;
+	newst->layer1 = &dev->D;
+	dev->D.recv = l1_receive;
+	dev->D.peer = &newst->own;
+	newst->own.st = newst;
+	newst->own.ctrl = st_own_ctrl;
+	newst->own.send = mISDN_queue_message;
+	newst->own.recv = mISDN_queue_message;
+	if (*debug & DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: st(%s)\n", __func__, newst->dev->name);
+	newst->notify = &done;
+	newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
+		newst->dev->name);
+	if (IS_ERR(newst->thread)) {
+		err = PTR_ERR(newst->thread);
+		printk(KERN_ERR
+			"mISDN:cannot create kernel thread for %s (%d)\n",
+			newst->dev->name, err);
+		delete_teimanager(dev->teimgr);
+		kfree(newst);
+	} else
+		wait_for_completion(&done);
+	return err;
+}
+
+int
+connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
+		u_int protocol, struct sockaddr_mISDN *adr)
+{
+	struct mISDN_sock	*msk = container_of(ch, struct mISDN_sock, ch);
+	struct channel_req	rq;
+	int			err;
+
+
+	if (*debug &  DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
+			__func__, dev->name, protocol, adr->dev, adr->channel,
+			 adr->sapi, adr->tei);
+	switch (protocol) {
+	case ISDN_P_NT_S0:
+	case ISDN_P_NT_E1:
+	case ISDN_P_TE_S0:
+	case ISDN_P_TE_E1:
+#ifdef PROTOCOL_CHECK
+		/* this should be enhanced */
+		if (!list_empty(&dev->D.st->layer2)
+			&& dev->D.protocol != protocol)
+			return -EBUSY;
+		if (!hlist_empty(&dev->D.st->l1sock.head)
+			&& dev->D.protocol != protocol)
+			return -EBUSY;
+#endif
+		ch->recv = mISDN_queue_message;
+		ch->peer = &dev->D.st->own;
+		ch->st = dev->D.st;
+		rq.protocol = protocol;
+		rq.adr.channel = 0;
+		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
+		printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
+		if (err)
+			return err;
+		write_lock_bh(&dev->D.st->l1sock.lock);
+		sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
+		write_unlock_bh(&dev->D.st->l1sock.lock);
+		break;
+	default:
+		return -ENOPROTOOPT;
+	}
+	return 0;
+}
+
+int
+connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
+    u_int protocol, struct sockaddr_mISDN *adr)
+{
+	struct channel_req	rq, rq2;
+	int			pmask, err;
+	struct Bprotocol	*bp;
+
+	if (*debug &  DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
+			__func__, dev->name, protocol,
+			adr->dev, adr->channel, adr->sapi,
+			adr->tei);
+	ch->st = dev->D.st;
+	pmask = 1 << (protocol & ISDN_P_B_MASK);
+	if (pmask & dev->Bprotocols) {
+		rq.protocol = protocol;
+		rq.adr = *adr;
+		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
+		if (err)
+			return err;
+		ch->recv = rq.ch->send;
+		ch->peer = rq.ch;
+		rq.ch->recv = ch->send;
+		rq.ch->peer = ch;
+		rq.ch->st = dev->D.st;
+	} else {
+		bp = get_Bprotocol4mask(pmask);
+		if (!bp)
+			return -ENOPROTOOPT;
+		rq2.protocol = protocol;
+		rq2.adr = *adr;
+		rq2.ch = ch;
+		err = bp->create(&rq2);
+		if (err)
+			return err;
+		ch->recv = rq2.ch->send;
+		ch->peer = rq2.ch;
+		rq2.ch->st = dev->D.st;
+		rq.protocol = rq2.protocol;
+		rq.adr = *adr;
+		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
+		if (err) {
+			rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
+			return err;
+		}
+		rq2.ch->recv = rq.ch->send;
+		rq2.ch->peer = rq.ch;
+		rq.ch->recv = rq2.ch->send;
+		rq.ch->peer = rq2.ch;
+		rq.ch->st = dev->D.st;
+	}
+	ch->protocol = protocol;
+	ch->nr = rq.ch->nr;
+	return 0;
+}
+
+int
+create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
+    u_int protocol, struct sockaddr_mISDN *adr)
+{
+	struct channel_req	rq;
+	int			err;
+
+	if (*debug &  DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
+			__func__, dev->name, protocol,
+			adr->dev, adr->channel, adr->sapi,
+			adr->tei);
+	rq.protocol = ISDN_P_TE_S0;
+	if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
+		rq.protocol = ISDN_P_TE_E1;
+	switch (protocol) {
+	case ISDN_P_LAPD_NT:
+		rq.protocol = ISDN_P_NT_S0;
+		if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
+			rq.protocol = ISDN_P_NT_E1;
+	case ISDN_P_LAPD_TE:
+#ifdef PROTOCOL_CHECK
+		/* this should be enhanced */
+		if (!list_empty(&dev->D.st->layer2)
+			&& dev->D.protocol != protocol)
+			return -EBUSY;
+		if (!hlist_empty(&dev->D.st->l1sock.head)
+			&& dev->D.protocol != protocol)
+			return -EBUSY;
+#endif
+		ch->recv = mISDN_queue_message;
+		ch->peer = &dev->D.st->own;
+		ch->st = dev->D.st;
+		rq.adr.channel = 0;
+		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
+		printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
+		if (err)
+			break;
+		rq.protocol = protocol;
+		rq.adr = *adr;
+		rq.ch = ch;
+		err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
+		printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
+		if (!err) {
+			if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
+				break;
+			add_layer2(rq.ch, dev->D.st);
+			rq.ch->recv = mISDN_queue_message;
+			rq.ch->peer = &dev->D.st->own;
+			rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
+		}
+		break;
+	default:
+		err = -EPROTONOSUPPORT;
+	}
+	return err;
+}
+
+void
+delete_channel(struct mISDNchannel *ch)
+{
+	struct mISDN_sock	*msk = container_of(ch, struct mISDN_sock, ch);
+	struct mISDNchannel	*pch;
+
+	if (!ch->st) {
+		printk(KERN_WARNING "%s: no stack\n", __func__);
+		return;
+	}
+	if (*debug & DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
+		    ch->st->dev->name, ch->protocol);
+	if (ch->protocol >= ISDN_P_B_START) {
+		if (ch->peer) {
+			ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
+			ch->peer = NULL;
+		}
+		return;
+	}
+	switch (ch->protocol) {
+	case ISDN_P_NT_S0:
+	case ISDN_P_TE_S0:
+	case ISDN_P_NT_E1:
+	case ISDN_P_TE_E1:
+		write_lock_bh(&ch->st->l1sock.lock);
+		sk_del_node_init(&msk->sk);
+		write_unlock_bh(&ch->st->l1sock.lock);
+		ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
+		break;
+	case ISDN_P_LAPD_TE:
+		pch = get_channel4id(ch->st, ch->nr);
+		if (pch) {
+			mutex_lock(&ch->st->lmutex);
+			list_del(&pch->list);
+			mutex_unlock(&ch->st->lmutex);
+			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
+			pch = ch->st->dev->teimgr;
+			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
+		} else
+			printk(KERN_WARNING "%s: no l2 channel\n",
+			    __func__);
+		break;
+	case ISDN_P_LAPD_NT:
+		pch = ch->st->dev->teimgr;
+		if (pch) {
+			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
+		} else
+			printk(KERN_WARNING "%s: no l2 channel\n",
+			    __func__);
+		break;
+	default:
+		break;
+	}
+	return;
+}
+
+void
+delete_stack(struct mISDNdevice *dev)
+{
+	struct mISDNstack	*st = dev->D.st;
+	DECLARE_COMPLETION_ONSTACK(done);
+
+	if (*debug & DEBUG_CORE_FUNC)
+		printk(KERN_DEBUG "%s: st(%s)\n", __func__,
+		    st->dev->name);
+	if (dev->teimgr)
+		delete_teimanager(dev->teimgr);
+	if (st->thread) {
+		if (st->notify) {
+			printk(KERN_WARNING "%s: notifier in use\n",
+			    __func__);
+				complete(st->notify);
+		}
+		st->notify = &done;
+		test_and_set_bit(mISDN_STACK_ABORT, &st->status);
+		test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
+		wake_up_interruptible(&st->workq);
+		wait_for_completion(&done);
+	}
+	if (!list_empty(&st->layer2))
+		printk(KERN_WARNING "%s: layer2 list not empty\n",
+		    __func__);
+	if (!hlist_empty(&st->l1sock.head))
+		printk(KERN_WARNING "%s: layer1 list not empty\n",
+		    __func__);
+	kfree(st);
+}
+
+void
+mISDN_initstack(u_int *dp)
+{
+	debug = dp;
+}
diff --git a/drivers/isdn/mISDN/tei.c b/drivers/isdn/mISDN/tei.c
new file mode 100644
index 00000000000..56a76a0ffdd
--- /dev/null
+++ b/drivers/isdn/mISDN/tei.c
@@ -0,0 +1,1340 @@
+/*
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include "layer2.h"
+#include <linux/random.h>
+#include "core.h"
+
+#define ID_REQUEST	1
+#define ID_ASSIGNED	2
+#define ID_DENIED	3
+#define ID_CHK_REQ	4
+#define ID_CHK_RES	5
+#define ID_REMOVE	6
+#define ID_VERIFY	7
+
+#define TEI_ENTITY_ID	0xf
+
+#define MGR_PH_ACTIVE	16
+#define MGR_PH_NOTREADY	17
+
+#define DATIMER_VAL	10000
+
+static 	u_int	*debug;
+
+static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL};
+static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL};
+static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL};
+
+enum {
+	ST_L1_DEACT,
+	ST_L1_DEACT_PENDING,
+	ST_L1_ACTIV,
+};
+#define DEACT_STATE_COUNT (ST_L1_ACTIV+1)
+
+static char *strDeactState[] =
+{
+	"ST_L1_DEACT",
+	"ST_L1_DEACT_PENDING",
+	"ST_L1_ACTIV",
+};
+
+enum {
+	EV_ACTIVATE,
+	EV_ACTIVATE_IND,
+	EV_DEACTIVATE,
+	EV_DEACTIVATE_IND,
+	EV_UI,
+	EV_DATIMER,
+};
+
+#define DEACT_EVENT_COUNT (EV_DATIMER+1)
+
+static char *strDeactEvent[] =
+{
+	"EV_ACTIVATE",
+	"EV_ACTIVATE_IND",
+	"EV_DEACTIVATE",
+	"EV_DEACTIVATE_IND",
+	"EV_UI",
+	"EV_DATIMER",
+};
+
+static void
+da_debug(struct FsmInst *fi, char *fmt, ...)
+{
+	struct manager	*mgr = fi->userdata;
+	va_list va;
+
+	if (!(*debug & DEBUG_L2_TEIFSM))
+		return;
+	va_start(va, fmt);
+	printk(KERN_DEBUG "mgr(%d): ", mgr->ch.st->dev->id);
+	vprintk(fmt, va);
+	printk("\n");
+	va_end(va);
+}
+
+static void
+da_activate(struct FsmInst *fi, int event, void *arg)
+{
+	struct manager	*mgr = fi->userdata;
+
+	if (fi->state == ST_L1_DEACT_PENDING)
+		mISDN_FsmDelTimer(&mgr->datimer, 1);
+	mISDN_FsmChangeState(fi, ST_L1_ACTIV);
+}
+
+static void
+da_deactivate_ind(struct FsmInst *fi, int event, void *arg)
+{
+	mISDN_FsmChangeState(fi, ST_L1_DEACT);
+}
+
+static void
+da_deactivate(struct FsmInst *fi, int event, void *arg)
+{
+	struct manager	*mgr = fi->userdata;
+	struct layer2	*l2;
+	u_long		flags;
+
+	read_lock_irqsave(&mgr->lock, flags);
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if (l2->l2m.state > ST_L2_4) {
+			/* have still activ TEI */
+			read_unlock_irqrestore(&mgr->lock, flags);
+			return;
+		}
+	}
+	read_unlock_irqrestore(&mgr->lock, flags);
+	/* All TEI are inactiv */
+	mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, NULL, 1);
+	mISDN_FsmChangeState(fi, ST_L1_DEACT_PENDING);
+}
+
+static void
+da_ui(struct FsmInst *fi, int event, void *arg)
+{
+	struct manager	*mgr = fi->userdata;
+
+	/* restart da timer */
+	mISDN_FsmDelTimer(&mgr->datimer, 2);
+	mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, NULL, 2);
+
+}
+
+static void
+da_timer(struct FsmInst *fi, int event, void *arg)
+{
+	struct manager	*mgr = fi->userdata;
+	struct layer2	*l2;
+	u_long		flags;
+
+	/* check again */
+	read_lock_irqsave(&mgr->lock, flags);
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if (l2->l2m.state > ST_L2_4) {
+			/* have still activ TEI */
+			read_unlock_irqrestore(&mgr->lock, flags);
+			mISDN_FsmChangeState(fi, ST_L1_ACTIV);
+			return;
+		}
+	}
+	read_unlock_irqrestore(&mgr->lock, flags);
+	/* All TEI are inactiv */
+	mISDN_FsmChangeState(fi, ST_L1_DEACT);
+	_queue_data(&mgr->ch, PH_DEACTIVATE_REQ, MISDN_ID_ANY, 0, NULL,
+	    GFP_ATOMIC);
+}
+
+static struct FsmNode DeactFnList[] =
+{
+	{ST_L1_DEACT, EV_ACTIVATE_IND, da_activate},
+	{ST_L1_ACTIV, EV_DEACTIVATE_IND, da_deactivate_ind},
+	{ST_L1_ACTIV, EV_DEACTIVATE, da_deactivate},
+	{ST_L1_DEACT_PENDING, EV_ACTIVATE, da_activate},
+	{ST_L1_DEACT_PENDING, EV_UI, da_ui},
+	{ST_L1_DEACT_PENDING, EV_DATIMER, da_timer},
+};
+
+enum {
+	ST_TEI_NOP,
+	ST_TEI_IDREQ,
+	ST_TEI_IDVERIFY,
+};
+
+#define TEI_STATE_COUNT (ST_TEI_IDVERIFY+1)
+
+static char *strTeiState[] =
+{
+	"ST_TEI_NOP",
+	"ST_TEI_IDREQ",
+	"ST_TEI_IDVERIFY",
+};
+
+enum {
+	EV_IDREQ,
+	EV_ASSIGN,
+	EV_ASSIGN_REQ,
+	EV_DENIED,
+	EV_CHKREQ,
+	EV_CHKRESP,
+	EV_REMOVE,
+	EV_VERIFY,
+	EV_TIMER,
+};
+
+#define TEI_EVENT_COUNT (EV_TIMER+1)
+
+static char *strTeiEvent[] =
+{
+	"EV_IDREQ",
+	"EV_ASSIGN",
+	"EV_ASSIGN_REQ",
+	"EV_DENIED",
+	"EV_CHKREQ",
+	"EV_CHKRESP",
+	"EV_REMOVE",
+	"EV_VERIFY",
+	"EV_TIMER",
+};
+
+static void
+tei_debug(struct FsmInst *fi, char *fmt, ...)
+{
+	struct teimgr	*tm = fi->userdata;
+	va_list va;
+
+	if (!(*debug & DEBUG_L2_TEIFSM))
+		return;
+	va_start(va, fmt);
+	printk(KERN_DEBUG "tei(%d): ", tm->l2->tei);
+	vprintk(fmt, va);
+	printk("\n");
+	va_end(va);
+}
+
+
+
+static int
+get_free_id(struct manager *mgr)
+{
+	u64		ids = 0;
+	int		i;
+	struct layer2	*l2;
+
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if (l2->ch.nr > 63) {
+			printk(KERN_WARNING
+			    "%s: more as 63 layer2 for one device\n",
+			    __func__);
+			return -EBUSY;
+		}
+		test_and_set_bit(l2->ch.nr, (u_long *)&ids);
+	}
+	for (i = 1; i < 64; i++)
+		if (!test_bit(i, (u_long *)&ids))
+			return i;
+	printk(KERN_WARNING "%s: more as 63 layer2 for one device\n",
+	    __func__);
+	return -EBUSY;
+}
+
+static int
+get_free_tei(struct manager *mgr)
+{
+	u64		ids = 0;
+	int		i;
+	struct layer2	*l2;
+
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if (l2->ch.nr == 0)
+			continue;
+		if ((l2->ch.addr & 0xff) != 0)
+			continue;
+		i = l2->ch.addr >> 8;
+		if (i < 64)
+			continue;
+		i -= 64;
+
+		test_and_set_bit(i, (u_long *)&ids);
+	}
+	for (i = 0; i < 64; i++)
+		if (!test_bit(i, (u_long *)&ids))
+			return i + 64;
+	printk(KERN_WARNING "%s: more as 63 dynamic tei for one device\n",
+	    __func__);
+	return -1;
+}
+
+static void
+teiup_create(struct manager *mgr, u_int prim, int len, void *arg)
+{
+	struct sk_buff	*skb;
+	struct mISDNhead *hh;
+	int		err;
+
+	skb = mI_alloc_skb(len, GFP_ATOMIC);
+	if (!skb)
+		return;
+	hh = mISDN_HEAD_P(skb);
+	hh->prim = prim;
+	hh->id = (mgr->ch.nr << 16) | mgr->ch.addr;
+	if (len)
+		memcpy(skb_put(skb, len), arg, len);
+	err = mgr->up->send(mgr->up, skb);
+	if (err) {
+		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
+		dev_kfree_skb(skb);
+	}
+}
+
+static u_int
+new_id(struct manager *mgr)
+{
+	u_int	id;
+
+	id = mgr->nextid++;
+	if (id == 0x7fff)
+		mgr->nextid = 1;
+	id <<= 16;
+	id |= GROUP_TEI << 8;
+	id |= TEI_SAPI;
+	return id;
+}
+
+static void
+do_send(struct manager *mgr)
+{
+	if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
+		return;
+
+	if (!test_and_set_bit(MGR_PH_NOTREADY, &mgr->options)) {
+		struct sk_buff	*skb = skb_dequeue(&mgr->sendq);
+
+		if (!skb) {
+			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
+			return;
+		}
+		mgr->lastid = mISDN_HEAD_ID(skb);
+		mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
+		if (mgr->ch.recv(mgr->ch.peer, skb)) {
+			dev_kfree_skb(skb);
+			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
+			mgr->lastid = MISDN_ID_NONE;
+		}
+	}
+}
+
+static void
+do_ack(struct manager *mgr, u_int id)
+{
+	if (test_bit(MGR_PH_NOTREADY, &mgr->options)) {
+		if (id == mgr->lastid) {
+			if (test_bit(MGR_PH_ACTIVE, &mgr->options)) {
+				struct sk_buff	*skb;
+
+				skb = skb_dequeue(&mgr->sendq);
+				if (skb) {
+					mgr->lastid = mISDN_HEAD_ID(skb);
+					if (!mgr->ch.recv(mgr->ch.peer, skb))
+						return;
+					dev_kfree_skb(skb);
+				}
+			}
+			mgr->lastid = MISDN_ID_NONE;
+			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
+		}
+	}
+}
+
+static void
+mgr_send_down(struct manager *mgr, struct sk_buff *skb)
+{
+	skb_queue_tail(&mgr->sendq, skb);
+	if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) {
+		_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
+		    NULL, GFP_KERNEL);
+	} else {
+		do_send(mgr);
+	}
+}
+
+static int
+dl_unit_data(struct manager *mgr, struct sk_buff *skb)
+{
+	if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) /* only net send UI */
+		return -EINVAL;
+	if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
+		_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
+		    NULL, GFP_KERNEL);
+	skb_push(skb, 3);
+	skb->data[0] = 0x02; /* SAPI 0 C/R = 1 */
+	skb->data[1] = 0xff; /* TEI 127 */
+	skb->data[2] = UI;   /* UI frame */
+	mISDN_HEAD_PRIM(skb) = PH_DATA_REQ;
+	mISDN_HEAD_ID(skb) = new_id(mgr);
+	skb_queue_tail(&mgr->sendq, skb);
+	do_send(mgr);
+	return 0;
+}
+
+unsigned int
+random_ri(void)
+{
+	u16 x;
+
+	get_random_bytes(&x, sizeof(x));
+	return x;
+}
+
+static struct layer2 *
+findtei(struct manager *mgr, int tei)
+{
+	struct layer2	*l2;
+	u_long		flags;
+
+	read_lock_irqsave(&mgr->lock, flags);
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if ((l2->sapi == 0) && (l2->tei > 0) &&
+		    (l2->tei != GROUP_TEI) && (l2->tei == tei))
+			goto done;
+	}
+	l2 = NULL;
+done:
+	read_unlock_irqrestore(&mgr->lock, flags);
+	return l2;
+}
+
+static void
+put_tei_msg(struct manager *mgr, u_char m_id, unsigned int ri, u_char tei)
+{
+	struct sk_buff *skb;
+	u_char bp[8];
+
+	bp[0] = (TEI_SAPI << 2);
+	if (test_bit(MGR_OPT_NETWORK, &mgr->options))
+		bp[0] |= 2; /* CR:=1 for net command */
+	bp[1] = (GROUP_TEI << 1) | 0x1;
+	bp[2] = UI;
+	bp[3] = TEI_ENTITY_ID;
+	bp[4] = ri >> 8;
+	bp[5] = ri & 0xff;
+	bp[6] = m_id;
+	bp[7] = (tei << 1) | 1;
+	skb = _alloc_mISDN_skb(PH_DATA_REQ, new_id(mgr),
+	    8, bp, GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_WARNING "%s: no skb for tei msg\n", __func__);
+		return;
+	}
+	mgr_send_down(mgr, skb);
+}
+
+static void
+tei_id_request(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+
+	if (tm->l2->tei != GROUP_TEI) {
+		tm->tei_m.printdebug(&tm->tei_m,
+			"assign request for allready assigned tei %d",
+			tm->l2->tei);
+		return;
+	}
+	tm->ri = random_ri();
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(&tm->tei_m,
+			"assign request ri %d", tm->ri);
+	put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
+	mISDN_FsmChangeState(fi, ST_TEI_IDREQ);
+	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 1);
+	tm->nval = 3;
+}
+
+static void
+tei_id_assign(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr	*tm = fi->userdata;
+	struct layer2	*l2;
+	u_char *dp = arg;
+	int ri, tei;
+
+	ri = ((unsigned int) *dp++ << 8);
+	ri += *dp++;
+	dp++;
+	tei = *dp >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity assign ri %d tei %d",
+			ri, tei);
+	l2 = findtei(tm->mgr, tei);
+	if (l2) {	/* same tei is in use */
+		if (ri != l2->tm->ri) {
+			tm->tei_m.printdebug(fi,
+				"possible duplicate assignment tei %d", tei);
+			tei_l2(l2, MDL_ERROR_RSP, 0);
+		}
+	} else if (ri == tm->ri) {
+		mISDN_FsmDelTimer(&tm->timer, 1);
+		mISDN_FsmChangeState(fi, ST_TEI_NOP);
+		tei_l2(tm->l2, MDL_ASSIGN_REQ, tei);
+	}
+}
+
+static void
+tei_id_test_dup(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr	*tm = fi->userdata;
+	struct layer2	*l2;
+	u_char *dp = arg;
+	int tei, ri;
+
+	ri = ((unsigned int) *dp++ << 8);
+	ri += *dp++;
+	dp++;
+	tei = *dp >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "foreign identity assign ri %d tei %d",
+			ri, tei);
+	l2 = findtei(tm->mgr, tei);
+	if (l2) {	/* same tei is in use */
+		if (ri != l2->tm->ri) {	/* and it wasn't our request */
+			tm->tei_m.printdebug(fi,
+				"possible duplicate assignment tei %d", tei);
+			mISDN_FsmEvent(&l2->tm->tei_m, EV_VERIFY, NULL);
+		}
+	}
+}
+
+static void
+tei_id_denied(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+	int ri, tei;
+
+	ri = ((unsigned int) *dp++ << 8);
+	ri += *dp++;
+	dp++;
+	tei = *dp >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity denied ri %d tei %d",
+			ri, tei);
+}
+
+static void
+tei_id_chk_req(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+	int tei;
+
+	tei = *(dp+3) >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity check req tei %d", tei);
+	if ((tm->l2->tei != GROUP_TEI) && ((tei == GROUP_TEI) ||
+	    (tei == tm->l2->tei))) {
+		mISDN_FsmDelTimer(&tm->timer, 4);
+		mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
+		put_tei_msg(tm->mgr, ID_CHK_RES, random_ri(), tm->l2->tei);
+	}
+}
+
+static void
+tei_id_remove(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+	int tei;
+
+	tei = *(dp+3) >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity remove tei %d", tei);
+	if ((tm->l2->tei != GROUP_TEI) &&
+	    ((tei == GROUP_TEI) || (tei == tm->l2->tei))) {
+		mISDN_FsmDelTimer(&tm->timer, 5);
+		mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
+		tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
+	}
+}
+
+static void
+tei_id_verify(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "id verify request for tei %d",
+			tm->l2->tei);
+	put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
+	mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
+	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
+	tm->nval = 2;
+}
+
+static void
+tei_id_req_tout(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+
+	if (--tm->nval) {
+		tm->ri = random_ri();
+		if (*debug & DEBUG_L2_TEI)
+			tm->tei_m.printdebug(fi, "assign req(%d) ri %d",
+				4 - tm->nval, tm->ri);
+		put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
+		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 3);
+	} else {
+		tm->tei_m.printdebug(fi, "assign req failed");
+		tei_l2(tm->l2, MDL_ERROR_RSP, 0);
+		mISDN_FsmChangeState(fi, ST_TEI_NOP);
+	}
+}
+
+static void
+tei_id_ver_tout(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+
+	if (--tm->nval) {
+		if (*debug & DEBUG_L2_TEI)
+			tm->tei_m.printdebug(fi,
+				"id verify req(%d) for tei %d",
+				3 - tm->nval, tm->l2->tei);
+		put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
+		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
+	} else {
+		tm->tei_m.printdebug(fi, "verify req for tei %d failed",
+			tm->l2->tei);
+		tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
+		mISDN_FsmChangeState(fi, ST_TEI_NOP);
+	}
+}
+
+static struct FsmNode TeiFnListUser[] =
+{
+	{ST_TEI_NOP, EV_IDREQ, tei_id_request},
+	{ST_TEI_NOP, EV_ASSIGN, tei_id_test_dup},
+	{ST_TEI_NOP, EV_VERIFY, tei_id_verify},
+	{ST_TEI_NOP, EV_REMOVE, tei_id_remove},
+	{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req},
+	{ST_TEI_IDREQ, EV_TIMER, tei_id_req_tout},
+	{ST_TEI_IDREQ, EV_ASSIGN, tei_id_assign},
+	{ST_TEI_IDREQ, EV_DENIED, tei_id_denied},
+	{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout},
+	{ST_TEI_IDVERIFY, EV_REMOVE, tei_id_remove},
+	{ST_TEI_IDVERIFY, EV_CHKREQ, tei_id_chk_req},
+};
+
+static void
+tei_l2remove(struct layer2 *l2)
+{
+	put_tei_msg(l2->tm->mgr, ID_REMOVE, 0, l2->tei);
+	tei_l2(l2, MDL_REMOVE_REQ, 0);
+	list_del(&l2->ch.list);
+	l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+}
+
+static void
+tei_assign_req(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+
+	if (tm->l2->tei == GROUP_TEI) {
+		tm->tei_m.printdebug(&tm->tei_m,
+			"net tei assign request without tei");
+		return;
+	}
+	tm->ri = ((unsigned int) *dp++ << 8);
+	tm->ri += *dp++;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(&tm->tei_m,
+			"net assign request ri %d teim %d", tm->ri, *dp);
+	put_tei_msg(tm->mgr, ID_ASSIGNED, tm->ri, tm->l2->tei);
+	mISDN_FsmChangeState(fi, ST_TEI_NOP);
+}
+
+static void
+tei_id_chk_req_net(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr	*tm = fi->userdata;
+
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "id check request for tei %d",
+		    tm->l2->tei);
+	tm->rcnt = 0;
+	put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
+	mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
+	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
+	tm->nval = 2;
+}
+
+static void
+tei_id_chk_resp(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+	int tei;
+
+	tei = dp[3] >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity check resp tei %d", tei);
+	if (tei == tm->l2->tei)
+		tm->rcnt++;
+}
+
+static void
+tei_id_verify_net(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+	u_char *dp = arg;
+	int tei;
+
+	tei = dp[3] >> 1;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(fi, "identity verify req tei %d/%d",
+		    tei, tm->l2->tei);
+	if (tei == tm->l2->tei)
+		tei_id_chk_req_net(fi, event, arg);
+}
+
+static void
+tei_id_ver_tout_net(struct FsmInst *fi, int event, void *arg)
+{
+	struct teimgr *tm = fi->userdata;
+
+	if (tm->rcnt == 1) {
+		if (*debug & DEBUG_L2_TEI)
+			tm->tei_m.printdebug(fi,
+			    "check req for tei %d sucessful\n", tm->l2->tei);
+		mISDN_FsmChangeState(fi, ST_TEI_NOP);
+	} else if (tm->rcnt > 1) {
+		/* duplicate assignment; remove */
+		tei_l2remove(tm->l2);
+	} else if (--tm->nval) {
+		if (*debug & DEBUG_L2_TEI)
+			tm->tei_m.printdebug(fi,
+				"id check req(%d) for tei %d",
+				3 - tm->nval, tm->l2->tei);
+		put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
+		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
+	} else {
+		tm->tei_m.printdebug(fi, "check req for tei %d failed",
+			tm->l2->tei);
+		mISDN_FsmChangeState(fi, ST_TEI_NOP);
+		tei_l2remove(tm->l2);
+	}
+}
+
+static struct FsmNode TeiFnListNet[] =
+{
+	{ST_TEI_NOP, EV_ASSIGN_REQ, tei_assign_req},
+	{ST_TEI_NOP, EV_VERIFY, tei_id_verify_net},
+	{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req_net},
+	{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout_net},
+	{ST_TEI_IDVERIFY, EV_CHKRESP, tei_id_chk_resp},
+};
+
+static void
+tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len)
+{
+	if (test_bit(FLG_FIXED_TEI, &tm->l2->flag))
+		return;
+	if (*debug & DEBUG_L2_TEI)
+		tm->tei_m.printdebug(&tm->tei_m, "tei handler mt %x", mt);
+	if (mt == ID_ASSIGNED)
+		mISDN_FsmEvent(&tm->tei_m, EV_ASSIGN, dp);
+	else if (mt == ID_DENIED)
+		mISDN_FsmEvent(&tm->tei_m, EV_DENIED, dp);
+	else if (mt == ID_CHK_REQ)
+		mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, dp);
+	else if (mt == ID_REMOVE)
+		mISDN_FsmEvent(&tm->tei_m, EV_REMOVE, dp);
+	else if (mt == ID_VERIFY)
+		mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, dp);
+	else if (mt == ID_CHK_RES)
+		mISDN_FsmEvent(&tm->tei_m, EV_CHKRESP, dp);
+}
+
+static struct layer2 *
+create_new_tei(struct manager *mgr, int tei)
+{
+	u_long		opt = 0;
+	u_long		flags;
+	int		id;
+	struct layer2	*l2;
+
+	if (!mgr->up)
+		return NULL;
+	if (tei < 64)
+		test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
+	if (mgr->ch.st->dev->Dprotocols
+	  & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
+		test_and_set_bit(OPTION_L2_PMX, &opt);
+	l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, (u_int)opt, (u_long)tei);
+	if (!l2) {
+		printk(KERN_WARNING "%s:no memory for layer2\n", __func__);
+		return NULL;
+	}
+	l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
+	if (!l2->tm) {
+		kfree(l2);
+		printk(KERN_WARNING "%s:no memory for teimgr\n", __func__);
+		return NULL;
+	}
+	l2->tm->mgr = mgr;
+	l2->tm->l2 = l2;
+	l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
+	l2->tm->tei_m.userdata = l2->tm;
+	l2->tm->tei_m.printdebug = tei_debug;
+	l2->tm->tei_m.fsm = &teifsmn;
+	l2->tm->tei_m.state = ST_TEI_NOP;
+	l2->tm->tval = 2000; /* T202  2 sec */
+	mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
+	write_lock_irqsave(&mgr->lock, flags);
+	id = get_free_id(mgr);
+	list_add_tail(&l2->list, &mgr->layer2);
+	write_unlock_irqrestore(&mgr->lock, flags);
+	if (id < 0) {
+		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+		printk(KERN_WARNING "%s:no free id\n", __func__);
+		return NULL;
+	} else {
+		l2->ch.nr = id;
+		__add_layer2(&l2->ch, mgr->ch.st);
+		l2->ch.recv = mgr->ch.recv;
+		l2->ch.peer = mgr->ch.peer;
+		l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
+	}
+	return l2;
+}
+
+static void
+new_tei_req(struct manager *mgr, u_char *dp)
+{
+	int		tei, ri;
+	struct layer2	*l2;
+
+	ri = dp[0] << 8;
+	ri += dp[1];
+	if (!mgr->up)
+		goto denied;
+	tei = get_free_tei(mgr);
+	if (tei < 0) {
+		printk(KERN_WARNING "%s:No free tei\n", __func__);
+		goto denied;
+	}
+	l2 = create_new_tei(mgr, tei);
+	if (!l2)
+		goto denied;
+	else
+		mISDN_FsmEvent(&l2->tm->tei_m, EV_ASSIGN_REQ, dp);
+	return;
+denied:
+	put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI);
+}
+
+static int
+ph_data_ind(struct manager *mgr, struct sk_buff *skb)
+{
+	int		ret = -EINVAL;
+	struct layer2	*l2;
+	u_long		flags;
+	u_char		mt;
+
+	if (skb->len < 8) {
+		if (*debug  & DEBUG_L2_TEI)
+			printk(KERN_DEBUG "%s: short mgr frame %d/8\n",
+			    __func__, skb->len);
+		goto done;
+	}
+	if (*debug  & DEBUG_L2_TEI)
+
+	if ((skb->data[0] >> 2) != TEI_SAPI) /* not for us */
+		goto done;
+	if (skb->data[0] & 1) /* EA0 formal error */
+		goto done;
+	if (!(skb->data[1] & 1)) /* EA1 formal error */
+		goto done;
+	if ((skb->data[1] >> 1) != GROUP_TEI) /* not for us */
+		goto done;
+	if ((skb->data[2] & 0xef) != UI) /* not UI */
+		goto done;
+	if (skb->data[3] != TEI_ENTITY_ID) /* not tei entity */
+		goto done;
+	mt = skb->data[6];
+	switch (mt) {
+	case ID_REQUEST:
+	case ID_CHK_RES:
+	case ID_VERIFY:
+		if (!test_bit(MGR_OPT_NETWORK, &mgr->options))
+			goto done;
+		break;
+	case ID_ASSIGNED:
+	case ID_DENIED:
+	case ID_CHK_REQ:
+	case ID_REMOVE:
+		if (test_bit(MGR_OPT_NETWORK, &mgr->options))
+			goto done;
+		break;
+	default:
+		goto done;
+	}
+	ret = 0;
+	if (mt == ID_REQUEST) {
+		new_tei_req(mgr, &skb->data[4]);
+		goto done;
+	}
+	read_lock_irqsave(&mgr->lock, flags);
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		tei_ph_data_ind(l2->tm, mt, &skb->data[4], skb->len - 4);
+	}
+	read_unlock_irqrestore(&mgr->lock, flags);
+done:
+	return ret;
+}
+
+int
+l2_tei(struct layer2 *l2, u_int cmd, u_long arg)
+{
+	struct teimgr	*tm = l2->tm;
+
+	if (test_bit(FLG_FIXED_TEI, &l2->flag))
+		return 0;
+	if (*debug & DEBUG_L2_TEI)
+		printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd);
+	switch (cmd) {
+	case MDL_ASSIGN_IND:
+		mISDN_FsmEvent(&tm->tei_m, EV_IDREQ, NULL);
+		break;
+	case MDL_ERROR_IND:
+		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
+			mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, &l2->tei);
+		if (test_bit(MGR_OPT_USER, &tm->mgr->options))
+			mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, NULL);
+		break;
+	case MDL_STATUS_UP_IND:
+		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
+			mISDN_FsmEvent(&tm->mgr->deact, EV_ACTIVATE, NULL);
+		break;
+	case MDL_STATUS_DOWN_IND:
+		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
+			mISDN_FsmEvent(&tm->mgr->deact, EV_DEACTIVATE, NULL);
+		break;
+	case MDL_STATUS_UI_IND:
+		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
+			mISDN_FsmEvent(&tm->mgr->deact, EV_UI, NULL);
+		break;
+	}
+	return 0;
+}
+
+void
+release_tei(struct layer2 *l2)
+{
+	struct teimgr	*tm = l2->tm;
+	u_long		flags;
+
+	mISDN_FsmDelTimer(&tm->timer, 1);
+	write_lock_irqsave(&tm->mgr->lock, flags);
+	list_del(&l2->list);
+	write_unlock_irqrestore(&tm->mgr->lock, flags);
+	l2->tm = NULL;
+	kfree(tm);
+}
+
+static int
+create_teimgr(struct manager *mgr, struct channel_req *crq)
+{
+	struct layer2	*l2;
+	u_long 		opt = 0;
+	u_long		flags;
+	int		id;
+
+	if (*debug & DEBUG_L2_TEI)
+		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
+			__func__, mgr->ch.st->dev->name, crq->protocol,
+			crq->adr.dev, crq->adr.channel, crq->adr.sapi,
+			crq->adr.tei);
+	if (crq->adr.sapi != 0) /* not supported yet */
+		return -EINVAL;
+	if (crq->adr.tei > GROUP_TEI)
+		return -EINVAL;
+	if (crq->adr.tei < 64)
+		test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
+	if (crq->adr.tei == 0)
+		test_and_set_bit(OPTION_L2_PTP, &opt);
+	if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
+		if (crq->protocol == ISDN_P_LAPD_TE)
+			return -EPROTONOSUPPORT;
+		if ((crq->adr.tei != 0) && (crq->adr.tei != 127))
+			return -EINVAL;
+		if (mgr->up) {
+			printk(KERN_WARNING
+			    "%s: only one network manager is allowed\n",
+			    __func__);
+			return -EBUSY;
+		}
+	} else if (test_bit(MGR_OPT_USER, &mgr->options)) {
+		if (crq->protocol == ISDN_P_LAPD_NT)
+			return -EPROTONOSUPPORT;
+		if ((crq->adr.tei >= 64) && (crq->adr.tei < GROUP_TEI))
+			return -EINVAL; /* dyn tei */
+	} else {
+		if (crq->protocol == ISDN_P_LAPD_NT)
+			test_and_set_bit(MGR_OPT_NETWORK, &mgr->options);
+		if (crq->protocol == ISDN_P_LAPD_TE)
+			test_and_set_bit(MGR_OPT_USER, &mgr->options);
+	}
+	if (mgr->ch.st->dev->Dprotocols
+	  & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
+		test_and_set_bit(OPTION_L2_PMX, &opt);
+	if ((crq->protocol == ISDN_P_LAPD_NT) && (crq->adr.tei == 127)) {
+		mgr->up = crq->ch;
+		id = DL_INFO_L2_CONNECT;
+		teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id);
+		crq->ch = NULL;
+		if (!list_empty(&mgr->layer2)) {
+			read_lock_irqsave(&mgr->lock, flags);
+			list_for_each_entry(l2, &mgr->layer2, list) {
+				l2->up = mgr->up;
+				l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
+			}
+			read_unlock_irqrestore(&mgr->lock, flags);
+		}
+		return 0;
+	}
+	l2 = create_l2(crq->ch, crq->protocol, (u_int)opt,
+		(u_long)crq->adr.tei);
+	if (!l2)
+		return -ENOMEM;
+	l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
+	if (!l2->tm) {
+		kfree(l2);
+		printk(KERN_ERR "kmalloc teimgr failed\n");
+		return -ENOMEM;
+	}
+	l2->tm->mgr = mgr;
+	l2->tm->l2 = l2;
+	l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
+	l2->tm->tei_m.userdata = l2->tm;
+	l2->tm->tei_m.printdebug = tei_debug;
+	if (crq->protocol == ISDN_P_LAPD_TE) {
+		l2->tm->tei_m.fsm = &teifsmu;
+		l2->tm->tei_m.state = ST_TEI_NOP;
+		l2->tm->tval = 1000; /* T201  1 sec */
+	} else {
+		l2->tm->tei_m.fsm = &teifsmn;
+		l2->tm->tei_m.state = ST_TEI_NOP;
+		l2->tm->tval = 2000; /* T202  2 sec */
+	}
+	mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
+	write_lock_irqsave(&mgr->lock, flags);
+	id = get_free_id(mgr);
+	list_add_tail(&l2->list, &mgr->layer2);
+	write_unlock_irqrestore(&mgr->lock, flags);
+	if (id < 0) {
+		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+	} else {
+		l2->ch.nr = id;
+		l2->up->nr = id;
+		crq->ch = &l2->ch;
+		id = 0;
+	}
+	return id;
+}
+
+static int
+mgr_send(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct manager	*mgr;
+	struct mISDNhead	*hh =  mISDN_HEAD_P(skb);
+	int			ret = -EINVAL;
+
+	mgr = container_of(ch, struct manager, ch);
+	if (*debug & DEBUG_L2_RECV)
+		printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
+		    __func__, hh->prim, hh->id);
+	switch (hh->prim) {
+	case PH_DATA_IND:
+		mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
+		ret = ph_data_ind(mgr, skb);
+		break;
+	case PH_DATA_CNF:
+		do_ack(mgr, hh->id);
+		ret = 0;
+		break;
+	case PH_ACTIVATE_IND:
+		test_and_set_bit(MGR_PH_ACTIVE, &mgr->options);
+		mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL);
+		do_send(mgr);
+		ret = 0;
+		break;
+	case PH_DEACTIVATE_IND:
+		test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options);
+		mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL);
+		ret = 0;
+		break;
+	case DL_UNITDATA_REQ:
+		return dl_unit_data(mgr, skb);
+	}
+	if (!ret)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int
+free_teimanager(struct manager *mgr)
+{
+	struct layer2	*l2, *nl2;
+
+	if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
+		/* not locked lock is taken in release tei */
+		mgr->up = NULL;
+		if (test_bit(OPTION_L2_CLEANUP, &mgr->options)) {
+			list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
+				put_tei_msg(mgr, ID_REMOVE, 0, l2->tei);
+				mutex_lock(&mgr->ch.st->lmutex);
+				list_del(&l2->ch.list);
+				mutex_unlock(&mgr->ch.st->lmutex);
+				l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+			}
+			test_and_clear_bit(MGR_OPT_NETWORK, &mgr->options);
+		} else {
+			list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
+				l2->up = NULL;
+			}
+		}
+	}
+	if (test_bit(MGR_OPT_USER, &mgr->options)) {
+		if (list_empty(&mgr->layer2))
+			test_and_clear_bit(MGR_OPT_USER, &mgr->options);
+	}
+	mgr->ch.st->dev->D.ctrl(&mgr->ch.st->dev->D, CLOSE_CHANNEL, NULL);
+	return 0;
+}
+
+static int
+ctrl_teimanager(struct manager *mgr, void *arg)
+{
+	/* currently we only have one option */
+	int	clean = *((int *)arg);
+
+	if (clean)
+		test_and_set_bit(OPTION_L2_CLEANUP, &mgr->options);
+	else
+		test_and_clear_bit(OPTION_L2_CLEANUP, &mgr->options);
+	return 0;
+}
+
+/* This function does create a L2 for fixed TEI in NT Mode */
+static int
+check_data(struct manager *mgr, struct sk_buff *skb)
+{
+	struct mISDNhead	*hh =  mISDN_HEAD_P(skb);
+	int			ret, tei;
+	struct layer2		*l2;
+
+	if (*debug & DEBUG_L2_CTRL)
+		printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
+		    __func__, hh->prim, hh->id);
+	if (test_bit(MGR_OPT_USER, &mgr->options))
+		return -ENOTCONN;
+	if (hh->prim != PH_DATA_IND)
+		return -ENOTCONN;
+	if (skb->len != 3)
+		return -ENOTCONN;
+	if (skb->data[0] != 0)
+		/* only SAPI 0 command */
+		return -ENOTCONN;
+	if (!(skb->data[1] & 1)) /* invalid EA1 */
+		return -EINVAL;
+	tei = skb->data[1] >> 0;
+	if (tei > 63) /* not a fixed tei */
+		return -ENOTCONN;
+	if ((skb->data[2] & ~0x10) != SABME)
+		return -ENOTCONN;
+	/* We got a SABME for a fixed TEI */
+	l2 = create_new_tei(mgr, tei);
+	if (!l2)
+		return -ENOMEM;
+	ret = l2->ch.send(&l2->ch, skb);
+	return ret;
+}
+
+void
+delete_teimanager(struct mISDNchannel *ch)
+{
+	struct manager	*mgr;
+	struct layer2	*l2, *nl2;
+
+	mgr = container_of(ch, struct manager, ch);
+	/* not locked lock is taken in release tei */
+	list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
+		mutex_lock(&mgr->ch.st->lmutex);
+		list_del(&l2->ch.list);
+		mutex_unlock(&mgr->ch.st->lmutex);
+		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+	}
+	list_del(&mgr->ch.list);
+	list_del(&mgr->bcast.list);
+	skb_queue_purge(&mgr->sendq);
+	kfree(mgr);
+}
+
+static int
+mgr_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+	struct manager	*mgr;
+	int		ret = -EINVAL;
+
+	mgr = container_of(ch, struct manager, ch);
+	if (*debug & DEBUG_L2_CTRL)
+		printk(KERN_DEBUG "%s(%x, %p)\n", __func__, cmd, arg);
+	switch (cmd) {
+	case OPEN_CHANNEL:
+		ret = create_teimgr(mgr, arg);
+		break;
+	case CLOSE_CHANNEL:
+		ret = free_teimanager(mgr);
+		break;
+	case CONTROL_CHANNEL:
+		ret = ctrl_teimanager(mgr, arg);
+		break;
+	case CHECK_DATA:
+		ret = check_data(mgr, arg);
+		break;
+	}
+	return ret;
+}
+
+static int
+mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb)
+{
+	struct manager		*mgr = container_of(ch, struct manager, bcast);
+	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	struct sk_buff		*cskb = NULL;
+	struct layer2		*l2;
+	u_long			flags;
+	int			ret;
+
+	read_lock_irqsave(&mgr->lock, flags);
+	list_for_each_entry(l2, &mgr->layer2, list) {
+		if ((hh->id & MISDN_ID_SAPI_MASK) ==
+		    (l2->ch.addr & MISDN_ID_SAPI_MASK)) {
+			if (list_is_last(&l2->list, &mgr->layer2)) {
+				cskb = skb;
+				skb = NULL;
+			} else {
+				if (!cskb)
+					cskb = skb_copy(skb, GFP_KERNEL);
+			}
+			if (cskb) {
+				ret = l2->ch.send(&l2->ch, cskb);
+				if (ret) {
+					if (*debug & DEBUG_SEND_ERR)
+						printk(KERN_DEBUG
+						    "%s ch%d prim(%x) addr(%x)"
+						    " err %d\n",
+						    __func__, l2->ch.nr,
+						    hh->prim, l2->ch.addr, ret);
+				} else
+					cskb = NULL;
+			} else {
+				printk(KERN_WARNING "%s ch%d addr %x no mem\n",
+				    __func__, ch->nr, ch->addr);
+				goto out;
+			}
+		}
+	}
+out:
+	read_unlock_irqrestore(&mgr->lock, flags);
+	if (cskb)
+		dev_kfree_skb(cskb);
+	if (skb)
+		dev_kfree_skb(skb);
+	return 0;
+}
+
+static int
+mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
+{
+
+	return -EINVAL;
+}
+
+int
+create_teimanager(struct mISDNdevice *dev)
+{
+	struct manager *mgr;
+
+	mgr = kzalloc(sizeof(struct manager), GFP_KERNEL);
+	if (!mgr)
+		return -ENOMEM;
+	INIT_LIST_HEAD(&mgr->layer2);
+	mgr->lock = __RW_LOCK_UNLOCKED(mgr->lock);
+	skb_queue_head_init(&mgr->sendq);
+	mgr->nextid = 1;
+	mgr->lastid = MISDN_ID_NONE;
+	mgr->ch.send = mgr_send;
+	mgr->ch.ctrl = mgr_ctrl;
+	mgr->ch.st = dev->D.st;
+	set_channel_address(&mgr->ch, TEI_SAPI, GROUP_TEI);
+	add_layer2(&mgr->ch, dev->D.st);
+	mgr->bcast.send = mgr_bcast;
+	mgr->bcast.ctrl = mgr_bcast_ctrl;
+	mgr->bcast.st = dev->D.st;
+	set_channel_address(&mgr->bcast, 0, GROUP_TEI);
+	add_layer2(&mgr->bcast, dev->D.st);
+	mgr->deact.debug = *debug & DEBUG_MANAGER;
+	mgr->deact.userdata = mgr;
+	mgr->deact.printdebug = da_debug;
+	mgr->deact.fsm = &deactfsm;
+	mgr->deact.state = ST_L1_DEACT;
+	mISDN_FsmInitTimer(&mgr->deact, &mgr->datimer);
+	dev->teimgr = &mgr->ch;
+	return 0;
+}
+
+int TEIInit(u_int *deb)
+{
+	debug = deb;
+	teifsmu.state_count = TEI_STATE_COUNT;
+	teifsmu.event_count = TEI_EVENT_COUNT;
+	teifsmu.strEvent = strTeiEvent;
+	teifsmu.strState = strTeiState;
+	mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser));
+	teifsmn.state_count = TEI_STATE_COUNT;
+	teifsmn.event_count = TEI_EVENT_COUNT;
+	teifsmn.strEvent = strTeiEvent;
+	teifsmn.strState = strTeiState;
+	mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet));
+	deactfsm.state_count =  DEACT_STATE_COUNT;
+	deactfsm.event_count = DEACT_EVENT_COUNT;
+	deactfsm.strEvent = strDeactEvent;
+	deactfsm.strState = strDeactState;
+	mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList));
+	return 0;
+}
+
+void TEIFree(void)
+{
+	mISDN_FsmFree(&teifsmu);
+	mISDN_FsmFree(&teifsmn);
+	mISDN_FsmFree(&deactfsm);
+}
diff --git a/drivers/isdn/mISDN/timerdev.c b/drivers/isdn/mISDN/timerdev.c
new file mode 100644
index 00000000000..b5fabc7019d
--- /dev/null
+++ b/drivers/isdn/mISDN/timerdev.c
@@ -0,0 +1,301 @@
+/*
+ *
+ * general timer device for using in ISDN stacks
+ *
+ * Author	Karsten Keil <kkeil@novell.com>
+ *
+ * Copyright 2008  by Karsten Keil <kkeil@novell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/poll.h>
+#include <linux/vmalloc.h>
+#include <linux/timer.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mISDNif.h>
+
+static int	*debug;
+
+
+struct mISDNtimerdev {
+	int			next_id;
+	struct list_head	pending;
+	struct list_head	expired;
+	wait_queue_head_t	wait;
+	u_int			work;
+	spinlock_t		lock; /* protect lists */
+};
+
+struct mISDNtimer {
+	struct list_head	list;
+	struct  mISDNtimerdev	*dev;
+	struct timer_list	tl;
+	int			id;
+};
+
+static int
+mISDN_open(struct inode *ino, struct file *filep)
+{
+	struct mISDNtimerdev	*dev;
+
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s(%p,%p)\n", __func__, ino, filep);
+	dev = kmalloc(sizeof(struct mISDNtimerdev) , GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+	dev->next_id = 1;
+	INIT_LIST_HEAD(&dev->pending);
+	INIT_LIST_HEAD(&dev->expired);
+	spin_lock_init(&dev->lock);
+	dev->work = 0;
+	init_waitqueue_head(&dev->wait);
+	filep->private_data = dev;
+	__module_get(THIS_MODULE);
+	return 0;
+}
+
+static int
+mISDN_close(struct inode *ino, struct file *filep)
+{
+	struct mISDNtimerdev	*dev = filep->private_data;
+	struct mISDNtimer	*timer, *next;
+
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s(%p,%p)\n", __func__, ino, filep);
+	list_for_each_entry_safe(timer, next, &dev->pending, list) {
+		del_timer(&timer->tl);
+		kfree(timer);
+	}
+	list_for_each_entry_safe(timer, next, &dev->expired, list) {
+		kfree(timer);
+	}
+	kfree(dev);
+	module_put(THIS_MODULE);
+	return 0;
+}
+
+static ssize_t
+mISDN_read(struct file *filep, char *buf, size_t count, loff_t *off)
+{
+	struct mISDNtimerdev	*dev = filep->private_data;
+	struct mISDNtimer	*timer;
+	u_long	flags;
+	int	ret = 0;
+
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s(%p, %p, %d, %p)\n", __func__,
+			filep, buf, (int)count, off);
+	if (*off != filep->f_pos)
+		return -ESPIPE;
+
+	if (list_empty(&dev->expired) && (dev->work == 0)) {
+		if (filep->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+		wait_event_interruptible(dev->wait, (dev->work ||
+		    !list_empty(&dev->expired)));
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+	}
+	if (count < sizeof(int))
+		return -ENOSPC;
+	if (dev->work)
+		dev->work = 0;
+	if (!list_empty(&dev->expired)) {
+		spin_lock_irqsave(&dev->lock, flags);
+		timer = (struct mISDNtimer *)dev->expired.next;
+		list_del(&timer->list);
+		spin_unlock_irqrestore(&dev->lock, flags);
+		if (put_user(timer->id, (int *)buf))
+			ret = -EFAULT;
+		else
+			ret = sizeof(int);
+		kfree(timer);
+	}
+	return ret;
+}
+
+static loff_t
+mISDN_llseek(struct file *filep, loff_t offset, int orig)
+{
+	return -ESPIPE;
+}
+
+static ssize_t
+mISDN_write(struct file *filep, const char *buf, size_t count, loff_t *off)
+{
+	return -EOPNOTSUPP;
+}
+
+static unsigned int
+mISDN_poll(struct file *filep, poll_table *wait)
+{
+	struct mISDNtimerdev	*dev = filep->private_data;
+	unsigned int		mask = POLLERR;
+
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s(%p, %p)\n", __func__, filep, wait);
+	if (dev) {
+		poll_wait(filep, &dev->wait, wait);
+		mask = 0;
+		if (dev->work || !list_empty(&dev->expired))
+			mask |= (POLLIN | POLLRDNORM);
+		if (*debug & DEBUG_TIMER)
+			printk(KERN_DEBUG "%s work(%d) empty(%d)\n", __func__,
+				dev->work, list_empty(&dev->expired));
+	}
+	return mask;
+}
+
+static void
+dev_expire_timer(struct mISDNtimer *timer)
+{
+	u_long			flags;
+
+	spin_lock_irqsave(&timer->dev->lock, flags);
+	list_del(&timer->list);
+	list_add_tail(&timer->list, &timer->dev->expired);
+	spin_unlock_irqrestore(&timer->dev->lock, flags);
+	wake_up_interruptible(&timer->dev->wait);
+}
+
+static int
+misdn_add_timer(struct mISDNtimerdev *dev, int timeout)
+{
+	int 			id;
+	u_long			flags;
+	struct mISDNtimer	*timer;
+
+	if (!timeout) {
+		dev->work = 1;
+		wake_up_interruptible(&dev->wait);
+		id = 0;
+	} else {
+		timer = kzalloc(sizeof(struct mISDNtimer), GFP_KERNEL);
+		if (!timer)
+			return -ENOMEM;
+		spin_lock_irqsave(&dev->lock, flags);
+		timer->id = dev->next_id++;
+		if (dev->next_id < 0)
+			dev->next_id = 1;
+		list_add_tail(&timer->list, &dev->pending);
+		spin_unlock_irqrestore(&dev->lock, flags);
+		timer->dev = dev;
+		timer->tl.data = (long)timer;
+		timer->tl.function = (void *) dev_expire_timer;
+		init_timer(&timer->tl);
+		timer->tl.expires = jiffies + ((HZ * (u_long)timeout) / 1000);
+		add_timer(&timer->tl);
+		id = timer->id;
+	}
+	return id;
+}
+
+static int
+misdn_del_timer(struct mISDNtimerdev *dev, int id)
+{
+	u_long			flags;
+	struct mISDNtimer	*timer;
+	int			ret = 0;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	list_for_each_entry(timer, &dev->pending, list) {
+		if (timer->id == id) {
+			list_del_init(&timer->list);
+			del_timer(&timer->tl);
+			ret = timer->id;
+			kfree(timer);
+			goto unlock;
+		}
+	}
+unlock:
+	spin_unlock_irqrestore(&dev->lock, flags);
+	return ret;
+}
+
+static int
+mISDN_ioctl(struct inode *inode, struct file *filep, unsigned int cmd,
+    unsigned long arg)
+{
+	struct mISDNtimerdev	*dev = filep->private_data;
+	int			id, tout, ret = 0;
+
+
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s(%p, %x, %lx)\n", __func__,
+		    filep, cmd, arg);
+	switch (cmd) {
+	case IMADDTIMER:
+		if (get_user(tout, (int __user *)arg)) {
+			ret = -EFAULT;
+			break;
+		}
+		id = misdn_add_timer(dev, tout);
+		if (*debug & DEBUG_TIMER)
+			printk(KERN_DEBUG "%s add %d id %d\n", __func__,
+			    tout, id);
+		if (id < 0) {
+			ret = id;
+			break;
+		}
+		if (put_user(id, (int __user *)arg))
+			ret = -EFAULT;
+		break;
+	case IMDELTIMER:
+		if (get_user(id, (int __user *)arg)) {
+			ret = -EFAULT;
+			break;
+		}
+		if (*debug & DEBUG_TIMER)
+			printk(KERN_DEBUG "%s del id %d\n", __func__, id);
+		id = misdn_del_timer(dev, id);
+		if (put_user(id, (int __user *)arg))
+			ret = -EFAULT;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static struct file_operations mISDN_fops = {
+	.llseek		= mISDN_llseek,
+	.read		= mISDN_read,
+	.write		= mISDN_write,
+	.poll		= mISDN_poll,
+	.ioctl		= mISDN_ioctl,
+	.open		= mISDN_open,
+	.release	= mISDN_close,
+};
+
+static struct miscdevice mISDNtimer = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "mISDNtimer",
+	.fops	= &mISDN_fops,
+};
+
+int
+mISDN_inittimer(int *deb)
+{
+	int	err;
+
+	debug = deb;
+	err = misc_register(&mISDNtimer);
+	if (err)
+		printk(KERN_WARNING "mISDN: Could not register timer device\n");
+	return err;
+}
+
+void mISDN_timer_cleanup(void)
+{
+	misc_deregister(&mISDNtimer);
+}
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 86a369bc57d..9556262dda5 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -103,6 +103,14 @@ config LEDS_HP6XX
 	  This option enables led support for the handheld
 	  HP Jornada 620/660/680/690.
 
+config LEDS_PCA9532
+	tristate "LED driver for PCA9532 dimmer"
+	depends on LEDS_CLASS && I2C && INPUT && EXPERIMENTAL
+	help
+	  This option enables support for NXP pca9532
+	  led controller. It is generally only usefull
+	  as a platform driver
+
 config LEDS_GPIO
 	tristate "LED Support for GPIO connected LEDs"
 	depends on LEDS_CLASS && GENERIC_GPIO
@@ -147,6 +155,14 @@ config LEDS_CLEVO_MAIL
 	  To compile this driver as a module, choose M here: the
 	  module will be called leds-clevo-mail.
 
+config LEDS_PCA955X
+	tristate "LED Support for PCA955x I2C chips"
+	depends on LEDS_CLASS && I2C
+	help
+	  This option enables support for LEDs connected to PCA955x
+	  LED driver chips accessed via the I2C bus.  Supported
+	  devices include PCA9550, PCA9551, PCA9552, and PCA9553.
+
 comment "LED Triggers"
 
 config LEDS_TRIGGERS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 973d626f5f4..ff7982b4456 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -16,11 +16,13 @@ obj-$(CONFIG_LEDS_WRAP)			+= leds-wrap.o
 obj-$(CONFIG_LEDS_H1940)		+= leds-h1940.o
 obj-$(CONFIG_LEDS_COBALT_QUBE)		+= leds-cobalt-qube.o
 obj-$(CONFIG_LEDS_COBALT_RAQ)		+= leds-cobalt-raq.o
+obj-$(CONFIG_LEDS_PCA9532)		+= leds-pca9532.o
 obj-$(CONFIG_LEDS_GPIO)			+= leds-gpio.o
 obj-$(CONFIG_LEDS_CM_X270)              += leds-cm-x270.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_HP6XX)		+= leds-hp6xx.o
 obj-$(CONFIG_LEDS_FSG)			+= leds-fsg.o
+obj-$(CONFIG_LEDS_PCA955X)		+= leds-pca955x.o
 
 # LED Triggers
 obj-$(CONFIG_LEDS_TRIGGER_TIMER)	+= ledtrig-timer.o
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index 0f242b3f09b..f910eaffe3a 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -111,16 +111,17 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trigger)
 			flags);
 		if (led_cdev->trigger->deactivate)
 			led_cdev->trigger->deactivate(led_cdev);
+		led_cdev->trigger = NULL;
 		led_set_brightness(led_cdev, LED_OFF);
 	}
 	if (trigger) {
 		write_lock_irqsave(&trigger->leddev_list_lock, flags);
 		list_add_tail(&led_cdev->trig_list, &trigger->led_cdevs);
 		write_unlock_irqrestore(&trigger->leddev_list_lock, flags);
+		led_cdev->trigger = trigger;
 		if (trigger->activate)
 			trigger->activate(led_cdev);
 	}
-	led_cdev->trigger = trigger;
 }
 EXPORT_SYMBOL_GPL(led_trigger_set);
 
diff --git a/drivers/leds/leds-atmel-pwm.c b/drivers/leds/leds-atmel-pwm.c
index 28db6c1444e..52297c3ab24 100644
--- a/drivers/leds/leds-atmel-pwm.c
+++ b/drivers/leds/leds-atmel-pwm.c
@@ -37,7 +37,7 @@ static int __init pwmled_probe(struct platform_device *pdev)
 {
 	const struct gpio_led_platform_data	*pdata;
 	struct pwmled				*leds;
-	unsigned				i;
+	int					i;
 	int					status;
 
 	pdata = pdev->dev.platform_data;
diff --git a/drivers/leds/leds-h1940.c b/drivers/leds/leds-h1940.c
index bcec4223038..73c70502168 100644
--- a/drivers/leds/leds-h1940.c
+++ b/drivers/leds/leds-h1940.c
@@ -23,7 +23,8 @@
 /*
  * Green led.
  */
-void h1940_greenled_set(struct led_classdev *led_dev, enum led_brightness value)
+static void h1940_greenled_set(struct led_classdev *led_dev,
+			       enum led_brightness value)
 {
 	switch (value) {
 	case LED_HALF:
@@ -52,7 +53,8 @@ static struct led_classdev h1940_greenled = {
 /*
  * Red led.
  */
-void h1940_redled_set(struct led_classdev *led_dev, enum led_brightness value)
+static void h1940_redled_set(struct led_classdev *led_dev,
+			     enum led_brightness value)
 {
 	switch (value) {
 	case LED_HALF:
@@ -82,7 +84,8 @@ static struct led_classdev h1940_redled = {
  * Blue led.
  * (it can only be blue flashing led)
  */
-void h1940_blueled_set(struct led_classdev *led_dev, enum led_brightness value)
+static void h1940_blueled_set(struct led_classdev *led_dev,
+			      enum led_brightness value)
 {
 	if (value) {
 		/* flashing Blue */
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
new file mode 100644
index 00000000000..4064d4f6b33
--- /dev/null
+++ b/drivers/leds/leds-pca9532.c
@@ -0,0 +1,337 @@
+/*
+ * pca9532.c - 16-bit Led dimmer
+ *
+ * Copyright (C) 2008 Riku Voipio <riku.voipio@movial.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * Datasheet: http://www.nxp.com/acrobat/datasheets/PCA9532_3.pdf
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/leds.h>
+#include <linux/input.h>
+#include <linux/mutex.h>
+#include <linux/leds-pca9532.h>
+
+static const unsigned short normal_i2c[] = { /*0x60,*/ I2C_CLIENT_END};
+I2C_CLIENT_INSMOD_1(pca9532);
+
+#define PCA9532_REG_PSC(i) (0x2+(i)*2)
+#define PCA9532_REG_PWM(i) (0x3+(i)*2)
+#define PCA9532_REG_LS0  0x6
+#define LED_REG(led) ((led>>2)+PCA9532_REG_LS0)
+#define LED_NUM(led) (led & 0x3)
+
+#define ldev_to_led(c)       container_of(c, struct pca9532_led, ldev)
+
+struct pca9532_data {
+	struct i2c_client *client;
+	struct pca9532_led leds[16];
+	struct mutex update_lock;
+	struct input_dev    *idev;
+	u8 pwm[2];
+	u8 psc[2];
+};
+
+static int pca9532_probe(struct i2c_client *client,
+	const struct i2c_device_id *id);
+static int pca9532_remove(struct i2c_client *client);
+
+static const struct i2c_device_id pca9532_id[] = {
+	{ "pca9532", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, pca9532_id);
+
+static struct i2c_driver pca9532_driver = {
+	.driver = {
+		.name   = "pca9532",
+	},
+	.probe  = pca9532_probe,
+	.remove = pca9532_remove,
+	.id_table = pca9532_id,
+};
+
+/* We have two pwm/blinkers, but 16 possible leds to drive. Additionaly,
+ * the clever Thecus people are using one pwm to drive the beeper. So,
+ * as a compromise we average one pwm to the values requested by all
+ * leds that are not ON/OFF.
+ * */
+static int pca9532_setpwm(struct i2c_client *client, int pwm, int blink,
+	enum led_brightness value)
+{
+	int a = 0, b = 0, i = 0;
+	struct pca9532_data *data = i2c_get_clientdata(client);
+	for (i = 0; i < 16; i++) {
+		if (data->leds[i].type == PCA9532_TYPE_LED &&
+			data->leds[i].state == PCA9532_PWM0+pwm) {
+				a++;
+				b += data->leds[i].ldev.brightness;
+		}
+	}
+	if (a == 0) {
+		dev_err(&client->dev,
+		"fear of division by zero %d/%d, wanted %d\n",
+			b, a, value);
+		return -EINVAL;
+	}
+	b = b/a;
+	if (b > 0xFF)
+		return -EINVAL;
+	mutex_lock(&data->update_lock);
+	data->pwm[pwm] = b;
+	i2c_smbus_write_byte_data(client, PCA9532_REG_PWM(pwm),
+		data->pwm[pwm]);
+	data->psc[pwm] = blink;
+	i2c_smbus_write_byte_data(client, PCA9532_REG_PSC(pwm),
+		data->psc[pwm]);
+	mutex_unlock(&data->update_lock);
+	return 0;
+}
+
+/* Set LED routing */
+static void pca9532_setled(struct pca9532_led *led)
+{
+	struct i2c_client *client = led->client;
+	struct pca9532_data *data = i2c_get_clientdata(client);
+	char reg;
+
+	mutex_lock(&data->update_lock);
+	reg = i2c_smbus_read_byte_data(client, LED_REG(led->id));
+	/* zero led bits */
+	reg = reg & ~(0x3<<LED_NUM(led->id)*2);
+	/* set the new value */
+	reg = reg | (led->state << LED_NUM(led->id)*2);
+	i2c_smbus_write_byte_data(client, LED_REG(led->id), reg);
+	mutex_unlock(&data->update_lock);
+}
+
+static void pca9532_set_brightness(struct led_classdev *led_cdev,
+	enum led_brightness value)
+{
+	int err = 0;
+	struct pca9532_led *led = ldev_to_led(led_cdev);
+
+	if (value == LED_OFF)
+		led->state = PCA9532_OFF;
+	else if (value == LED_FULL)
+		led->state = PCA9532_ON;
+	else {
+		led->state = PCA9532_PWM0; /* Thecus: hardcode one pwm */
+		err = pca9532_setpwm(led->client, 0, 0, value);
+		if (err)
+			return; /* XXX: led api doesn't allow error code? */
+	}
+	pca9532_setled(led);
+}
+
+static int pca9532_set_blink(struct led_classdev *led_cdev,
+	unsigned long *delay_on, unsigned long *delay_off)
+{
+	struct pca9532_led *led = ldev_to_led(led_cdev);
+	struct i2c_client *client = led->client;
+	int psc;
+
+	if (*delay_on == 0 && *delay_off == 0) {
+	/* led subsystem ask us for a blink rate */
+		*delay_on  = 1000;
+		*delay_off = 1000;
+	}
+	if (*delay_on != *delay_off || *delay_on > 1690 || *delay_on < 6)
+		return -EINVAL;
+
+	/* Thecus specific: only use PSC/PWM 0 */
+	psc = (*delay_on * 152-1)/1000;
+	return pca9532_setpwm(client, 0, psc, led_cdev->brightness);
+}
+
+int pca9532_event(struct input_dev *dev, unsigned int type, unsigned int code,
+	int value)
+{
+	struct pca9532_data *data = input_get_drvdata(dev);
+
+	if (type != EV_SND && (code != SND_BELL || code != SND_TONE))
+		return -1;
+
+	/* XXX: allow different kind of beeps with psc/pwm modifications */
+	if (value > 1 && value < 32767)
+		data->pwm[1] = 127;
+	else
+		data->pwm[1] = 0;
+
+	dev_info(&dev->dev, "setting beep to %d \n", data->pwm[1]);
+	mutex_lock(&data->update_lock);
+	i2c_smbus_write_byte_data(data->client, PCA9532_REG_PWM(1),
+		data->pwm[1]);
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static int pca9532_configure(struct i2c_client *client,
+	struct pca9532_data *data, struct pca9532_platform_data *pdata)
+{
+	int i, err = 0;
+
+	for (i = 0; i < 2; i++)	{
+		data->pwm[i] = pdata->pwm[i];
+		data->psc[i] = pdata->psc[i];
+		i2c_smbus_write_byte_data(client, PCA9532_REG_PWM(i),
+			data->pwm[i]);
+		i2c_smbus_write_byte_data(client, PCA9532_REG_PSC(i),
+			data->psc[i]);
+	}
+
+	for (i = 0; i < 16; i++) {
+		struct pca9532_led *led = &data->leds[i];
+		struct pca9532_led *pled = &pdata->leds[i];
+		led->client = client;
+		led->id = i;
+		led->type = pled->type;
+		switch (led->type) {
+		case PCA9532_TYPE_NONE:
+			break;
+		case PCA9532_TYPE_LED:
+			led->state = pled->state;
+			led->name =  pled->name;
+			led->ldev.name = led->name;
+			led->ldev.brightness = LED_OFF;
+			led->ldev.brightness_set = pca9532_set_brightness;
+			led->ldev.blink_set = pca9532_set_blink;
+			if (led_classdev_register(&client->dev,
+				&led->ldev) < 0)	{
+				dev_err(&client->dev,
+					"couldn't register LED %s\n",
+					led->name);
+				goto exit;
+			}
+			pca9532_setled(led);
+			break;
+		case PCA9532_TYPE_N2100_BEEP:
+			BUG_ON(data->idev);
+			led->state = PCA9532_PWM1;
+			pca9532_setled(led);
+			data->idev = input_allocate_device();
+			if (data->idev == NULL) {
+				err = -ENOMEM;
+				goto exit;
+			}
+			data->idev->name = pled->name;
+			data->idev->phys = "i2c/pca9532";
+			data->idev->id.bustype = BUS_HOST;
+			data->idev->id.vendor  = 0x001f;
+			data->idev->id.product = 0x0001;
+			data->idev->id.version = 0x0100;
+			data->idev->evbit[0] = BIT_MASK(EV_SND);
+			data->idev->sndbit[0] = BIT_MASK(SND_BELL) |
+						BIT_MASK(SND_TONE);
+			data->idev->event = pca9532_event;
+			input_set_drvdata(data->idev, data);
+			err = input_register_device(data->idev);
+			if (err) {
+				input_free_device(data->idev);
+				data->idev = NULL;
+				goto exit;
+			}
+			break;
+		}
+	}
+	return 0;
+
+exit:
+	if (i > 0)
+		for (i = i - 1; i >= 0; i--)
+			switch (data->leds[i].type) {
+			case PCA9532_TYPE_NONE:
+				break;
+			case PCA9532_TYPE_LED:
+				led_classdev_unregister(&data->leds[i].ldev);
+				break;
+			case PCA9532_TYPE_N2100_BEEP:
+				if (data->idev != NULL) {
+					input_unregister_device(data->idev);
+					input_free_device(data->idev);
+					data->idev = NULL;
+				}
+				break;
+			}
+
+	return err;
+
+}
+
+static int pca9532_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct pca9532_data *data = i2c_get_clientdata(client);
+	struct pca9532_platform_data *pca9532_pdata = client->dev.platform_data;
+
+	if (!i2c_check_functionality(client->adapter,
+		I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	data = kzalloc(sizeof(struct pca9532_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	dev_info(&client->dev, "setting platform data\n");
+	i2c_set_clientdata(client, data);
+	data->client = client;
+	mutex_init(&data->update_lock);
+
+	if (pca9532_pdata == NULL)
+		return -EIO;
+
+	pca9532_configure(client, data, pca9532_pdata);
+	return 0;
+
+}
+
+static int pca9532_remove(struct i2c_client *client)
+{
+	struct pca9532_data *data = i2c_get_clientdata(client);
+	int i;
+	for (i = 0; i < 16; i++)
+		switch (data->leds[i].type) {
+		case PCA9532_TYPE_NONE:
+			break;
+		case PCA9532_TYPE_LED:
+			led_classdev_unregister(&data->leds[i].ldev);
+			break;
+		case PCA9532_TYPE_N2100_BEEP:
+			if (data->idev != NULL) {
+				input_unregister_device(data->idev);
+				input_free_device(data->idev);
+				data->idev = NULL;
+			}
+			break;
+		}
+
+	kfree(data);
+	i2c_set_clientdata(client, NULL);
+	return 0;
+}
+
+static int __init pca9532_init(void)
+{
+	return i2c_add_driver(&pca9532_driver);
+}
+
+static void __exit pca9532_exit(void)
+{
+	i2c_del_driver(&pca9532_driver);
+}
+
+MODULE_AUTHOR("Riku Voipio <riku.voipio@movial.fi>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("PCA 9532 LED dimmer");
+
+module_init(pca9532_init);
+module_exit(pca9532_exit);
+
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
new file mode 100644
index 00000000000..146c0697286
--- /dev/null
+++ b/drivers/leds/leds-pca955x.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright 2007-2008 Extreme Engineering Solutions, Inc.
+ *
+ * Author: Nate Case <ncase@xes-inc.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * LED driver for various PCA955x I2C LED drivers
+ *
+ * Supported devices:
+ *
+ *	Device		Description		7-bit slave address
+ *	------		-----------		-------------------
+ *	PCA9550		2-bit driver		0x60 .. 0x61
+ *	PCA9551		8-bit driver		0x60 .. 0x67
+ *	PCA9552		16-bit driver		0x60 .. 0x67
+ *	PCA9553/01	4-bit driver		0x62
+ *	PCA9553/02	4-bit driver		0x63
+ *
+ * Philips PCA955x LED driver chips follow a register map as shown below:
+ *
+ *	Control Register		Description
+ *	----------------		-----------
+ *	0x0				Input register 0
+ *					..
+ *	NUM_INPUT_REGS - 1		Last Input register X
+ *
+ *	NUM_INPUT_REGS			Frequency prescaler 0
+ *	NUM_INPUT_REGS + 1		PWM register 0
+ *	NUM_INPUT_REGS + 2		Frequency prescaler 1
+ *	NUM_INPUT_REGS + 3		PWM register 1
+ *
+ *	NUM_INPUT_REGS + 4		LED selector 0
+ *	NUM_INPUT_REGS + 4
+ *	    + NUM_LED_REGS - 1		Last LED selector
+ *
+ *  where NUM_INPUT_REGS and NUM_LED_REGS vary depending on how many
+ *  bits the chip supports.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+#include <linux/leds.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/workqueue.h>
+
+/* LED select registers determine the source that drives LED outputs */
+#define PCA955X_LS_LED_ON	0x0	/* Output LOW */
+#define PCA955X_LS_LED_OFF	0x1	/* Output HI-Z */
+#define PCA955X_LS_BLINK0	0x2	/* Blink at PWM0 rate */
+#define PCA955X_LS_BLINK1	0x3	/* Blink at PWM1 rate */
+
+enum pca955x_type {
+	pca9550,
+	pca9551,
+	pca9552,
+	pca9553,
+};
+
+struct pca955x_chipdef {
+	int			bits;
+	u8			slv_addr;	/* 7-bit slave address mask */
+	int			slv_addr_shift;	/* Number of bits to ignore */
+};
+
+static struct pca955x_chipdef pca955x_chipdefs[] = {
+	[pca9550] = {
+		.bits		= 2,
+		.slv_addr	= /* 110000x */ 0x60,
+		.slv_addr_shift	= 1,
+	},
+	[pca9551] = {
+		.bits		= 8,
+		.slv_addr	= /* 1100xxx */ 0x60,
+		.slv_addr_shift	= 3,
+	},
+	[pca9552] = {
+		.bits		= 16,
+		.slv_addr	= /* 1100xxx */ 0x60,
+		.slv_addr_shift	= 3,
+	},
+	[pca9553] = {
+		.bits		= 4,
+		.slv_addr	= /* 110001x */ 0x62,
+		.slv_addr_shift	= 1,
+	},
+};
+
+static const struct i2c_device_id pca955x_id[] = {
+	{ "pca9550", pca9550 },
+	{ "pca9551", pca9551 },
+	{ "pca9552", pca9552 },
+	{ "pca9553", pca9553 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, pca955x_id);
+
+struct pca955x_led {
+	struct pca955x_chipdef	*chipdef;
+	struct i2c_client	*client;
+	struct work_struct	work;
+	spinlock_t		lock;
+	enum led_brightness	brightness;
+	struct led_classdev	led_cdev;
+	int			led_num;	/* 0 .. 15 potentially */
+	char			name[32];
+};
+
+/* 8 bits per input register */
+static inline int pca95xx_num_input_regs(int bits)
+{
+	return (bits + 7) / 8;
+}
+
+/* 4 bits per LED selector register */
+static inline int pca95xx_num_led_regs(int bits)
+{
+	return (bits + 3)  / 4;
+}
+
+/*
+ * Return an LED selector register value based on an existing one, with
+ * the appropriate 2-bit state value set for the given LED number (0-3).
+ */
+static inline u8 pca955x_ledsel(u8 oldval, int led_num, int state)
+{
+	return (oldval & (~(0x3 << (led_num << 1)))) |
+		((state & 0x3) << (led_num << 1));
+}
+
+/*
+ * Write to frequency prescaler register, used to program the
+ * period of the PWM output.  period = (PSCx + 1) / 38
+ */
+static void pca955x_write_psc(struct i2c_client *client, int n, u8 val)
+{
+	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+
+	i2c_smbus_write_byte_data(client,
+		pca95xx_num_input_regs(pca955x->chipdef->bits) + 2*n,
+		val);
+}
+
+/*
+ * Write to PWM register, which determines the duty cycle of the
+ * output.  LED is OFF when the count is less than the value of this
+ * register, and ON when it is greater.  If PWMx == 0, LED is always OFF.
+ *
+ * Duty cycle is (256 - PWMx) / 256
+ */
+static void pca955x_write_pwm(struct i2c_client *client, int n, u8 val)
+{
+	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+
+	i2c_smbus_write_byte_data(client,
+		pca95xx_num_input_regs(pca955x->chipdef->bits) + 1 + 2*n,
+		val);
+}
+
+/*
+ * Write to LED selector register, which determines the source that
+ * drives the LED output.
+ */
+static void pca955x_write_ls(struct i2c_client *client, int n, u8 val)
+{
+	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+
+	i2c_smbus_write_byte_data(client,
+		pca95xx_num_input_regs(pca955x->chipdef->bits) + 4 + n,
+		val);
+}
+
+/*
+ * Read the LED selector register, which determines the source that
+ * drives the LED output.
+ */
+static u8 pca955x_read_ls(struct i2c_client *client, int n)
+{
+	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+
+	return (u8) i2c_smbus_read_byte_data(client,
+		pca95xx_num_input_regs(pca955x->chipdef->bits) + 4 + n);
+}
+
+static void pca955x_led_work(struct work_struct *work)
+{
+	struct pca955x_led *pca955x;
+	u8 ls;
+	int chip_ls;	/* which LSx to use (0-3 potentially) */
+	int ls_led;	/* which set of bits within LSx to use (0-3) */
+
+	pca955x = container_of(work, struct pca955x_led, work);
+	chip_ls = pca955x->led_num / 4;
+	ls_led = pca955x->led_num % 4;
+
+	ls = pca955x_read_ls(pca955x->client, chip_ls);
+
+	switch (pca955x->brightness) {
+	case LED_FULL:
+		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_LED_ON);
+		break;
+	case LED_OFF:
+		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_LED_OFF);
+		break;
+	case LED_HALF:
+		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_BLINK0);
+		break;
+	default:
+		/*
+		 * Use PWM1 for all other values.  This has the unwanted
+		 * side effect of making all LEDs on the chip share the
+		 * same brightness level if set to a value other than
+		 * OFF, HALF, or FULL.  But, this is probably better than
+		 * just turning off for all other values.
+		 */
+		pca955x_write_pwm(pca955x->client, 1, 255-pca955x->brightness);
+		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_BLINK1);
+		break;
+	}
+
+	pca955x_write_ls(pca955x->client, chip_ls, ls);
+}
+
+void pca955x_led_set(struct led_classdev *led_cdev, enum led_brightness value)
+{
+	struct pca955x_led *pca955x;
+
+	pca955x = container_of(led_cdev, struct pca955x_led, led_cdev);
+
+	spin_lock(&pca955x->lock);
+	pca955x->brightness = value;
+
+	/*
+	 * Must use workqueue for the actual I/O since I2C operations
+	 * can sleep.
+	 */
+	schedule_work(&pca955x->work);
+
+	spin_unlock(&pca955x->lock);
+}
+
+static int __devinit pca955x_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct pca955x_led *pca955x;
+	int i;
+	int err = -ENODEV;
+	struct pca955x_chipdef *chip;
+	struct i2c_adapter *adapter;
+	struct led_platform_data *pdata;
+
+	chip = &pca955x_chipdefs[id->driver_data];
+	adapter = to_i2c_adapter(client->dev.parent);
+	pdata = client->dev.platform_data;
+
+	/* Make sure the slave address / chip type combo given is possible */
+	if ((client->addr & ~((1 << chip->slv_addr_shift) - 1)) !=
+	    chip->slv_addr) {
+		dev_err(&client->dev, "invalid slave address %02x\n",
+				client->addr);
+		return -ENODEV;
+	}
+
+	printk(KERN_INFO "leds-pca955x: Using %s %d-bit LED driver at "
+			"slave address 0x%02x\n",
+			id->name, chip->bits, client->addr);
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -EIO;
+
+	if (pdata) {
+		if (pdata->num_leds != chip->bits) {
+			dev_err(&client->dev, "board info claims %d LEDs"
+					" on a %d-bit chip\n",
+					pdata->num_leds, chip->bits);
+			return -ENODEV;
+		}
+	}
+
+	for (i = 0; i < chip->bits; i++) {
+		pca955x = kzalloc(sizeof(struct pca955x_led), GFP_KERNEL);
+		if (!pca955x) {
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		pca955x->chipdef = chip;
+		pca955x->client = client;
+		pca955x->led_num = i;
+		/* Platform data can specify LED names and default triggers */
+		if (pdata) {
+			if (pdata->leds[i].name)
+				snprintf(pca955x->name, 32, "pca955x:%s",
+							pdata->leds[i].name);
+			if (pdata->leds[i].default_trigger)
+				pca955x->led_cdev.default_trigger =
+					pdata->leds[i].default_trigger;
+		} else {
+			snprintf(pca955x->name, 32, "pca955x:%d", i);
+		}
+		spin_lock_init(&pca955x->lock);
+
+		pca955x->led_cdev.name = pca955x->name;
+		pca955x->led_cdev.brightness_set =
+				pca955x_led_set;
+
+		/*
+		 * Client data is a pointer to the _first_ pca955x_led
+		 * struct
+		 */
+		if (i == 0)
+			i2c_set_clientdata(client, pca955x);
+
+		INIT_WORK(&(pca955x->work), pca955x_led_work);
+
+		led_classdev_register(&client->dev, &(pca955x->led_cdev));
+	}
+
+	/* Turn off LEDs */
+	for (i = 0; i < pca95xx_num_led_regs(chip->bits); i++)
+		pca955x_write_ls(client, i, 0x55);
+
+	/* PWM0 is used for half brightness or 50% duty cycle */
+	pca955x_write_pwm(client, 0, 255-LED_HALF);
+
+	/* PWM1 is used for variable brightness, default to OFF */
+	pca955x_write_pwm(client, 1, 0);
+
+	/* Set to fast frequency so we do not see flashing */
+	pca955x_write_psc(client, 0, 0);
+	pca955x_write_psc(client, 1, 0);
+
+	return 0;
+exit:
+	return err;
+}
+
+static int __devexit pca955x_remove(struct i2c_client *client)
+{
+	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	int leds = pca955x->chipdef->bits;
+	int i;
+
+	for (i = 0; i < leds; i++) {
+		led_classdev_unregister(&(pca955x->led_cdev));
+		cancel_work_sync(&(pca955x->work));
+		kfree(pca955x);
+		pca955x = pca955x + 1;
+	}
+
+	return 0;
+}
+
+static struct i2c_driver pca955x_driver = {
+	.driver = {
+		.name	= "leds-pca955x",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= pca955x_probe,
+	.remove	= __devexit_p(pca955x_remove),
+	.id_table = pca955x_id,
+};
+
+static int __init pca955x_leds_init(void)
+{
+	return i2c_add_driver(&pca955x_driver);
+}
+
+static void __exit pca955x_leds_exit(void)
+{
+	i2c_del_driver(&pca955x_driver);
+}
+
+module_init(pca955x_leds_init);
+module_exit(pca955x_leds_exit);
+
+MODULE_AUTHOR("Nate Case <ncase@xes-inc.com>");
+MODULE_DESCRIPTION("PCA955x LED driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/lguest/lguest_device.c b/drivers/lguest/lguest_device.c
index 1a8de57289e..37344aaee22 100644
--- a/drivers/lguest/lguest_device.c
+++ b/drivers/lguest/lguest_device.c
@@ -98,16 +98,20 @@ static u32 lg_get_features(struct virtio_device *vdev)
 	return features;
 }
 
-static void lg_set_features(struct virtio_device *vdev, u32 features)
+static void lg_finalize_features(struct virtio_device *vdev)
 {
-	unsigned int i;
+	unsigned int i, bits;
 	struct lguest_device_desc *desc = to_lgdev(vdev)->desc;
 	/* Second half of bitmap is features we accept. */
 	u8 *out_features = lg_features(desc) + desc->feature_len;
 
+	/* Give virtio_ring a chance to accept features. */
+	vring_transport_features(vdev);
+
 	memset(out_features, 0, desc->feature_len);
-	for (i = 0; i < min(desc->feature_len * 8, 32); i++) {
-		if (features & (1 << i))
+	bits = min_t(unsigned, desc->feature_len, sizeof(vdev->features)) * 8;
+	for (i = 0; i < bits; i++) {
+		if (test_bit(i, vdev->features))
 			out_features[i / 8] |= (1 << (i % 8));
 	}
 }
@@ -297,7 +301,7 @@ static void lg_del_vq(struct virtqueue *vq)
 /* The ops structure which hooks everything together. */
 static struct virtio_config_ops lguest_config_ops = {
 	.get_features = lg_get_features,
-	.set_features = lg_set_features,
+	.finalize_features = lg_finalize_features,
 	.get = lg_get,
 	.set = lg_set,
 	.get_status = lg_get_status,
diff --git a/drivers/media/dvb/pluto2/pluto2.c b/drivers/media/dvb/pluto2/pluto2.c
index 1360403b88b..a9653c63f4d 100644
--- a/drivers/media/dvb/pluto2/pluto2.c
+++ b/drivers/media/dvb/pluto2/pluto2.c
@@ -242,7 +242,7 @@ static int __devinit pluto_dma_map(struct pluto *pluto)
 	pluto->dma_addr = pci_map_single(pluto->pdev, pluto->dma_buf,
 			TS_DMA_BYTES, PCI_DMA_FROMDEVICE);
 
-	return pci_dma_mapping_error(pluto->dma_addr);
+	return pci_dma_mapping_error(pluto->pdev, pluto->dma_addr);
 }
 
 static void pluto_dma_unmap(struct pluto *pluto)
diff --git a/drivers/media/video/pvrusb2/pvrusb2-dvb.c b/drivers/media/video/pvrusb2/pvrusb2-dvb.c
index 6ec4bf81fc7..77b3c338506 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-dvb.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-dvb.c
@@ -20,6 +20,7 @@
 
 #include <linux/kthread.h>
 #include <linux/freezer.h>
+#include <linux/mm.h>
 #include "dvbdev.h"
 #include "pvrusb2-debug.h"
 #include "pvrusb2-hdw-internal.h"
diff --git a/drivers/media/video/pvrusb2/pvrusb2-ioread.c b/drivers/media/video/pvrusb2/pvrusb2-ioread.c
index 05a1376405e..b4824782d85 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-ioread.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-ioread.c
@@ -22,6 +22,7 @@
 #include "pvrusb2-debug.h"
 #include <linux/errno.h>
 #include <linux/string.h>
+#include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <asm/uaccess.h>
diff --git a/drivers/media/video/uvc/uvc_queue.c b/drivers/media/video/uvc/uvc_queue.c
index 7388d0cee3d..5646a6a3293 100644
--- a/drivers/media/video/uvc/uvc_queue.c
+++ b/drivers/media/video/uvc/uvc_queue.c
@@ -13,6 +13,7 @@
 
 #include <linux/kernel.h>
 #include <linux/version.h>
+#include <linux/mm.h>
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/usb.h>
diff --git a/drivers/media/video/videobuf-core.c b/drivers/media/video/videobuf-core.c
index 0a88c44ace0..b7b05842cf2 100644
--- a/drivers/media/video/videobuf-core.c
+++ b/drivers/media/video/videobuf-core.c
@@ -16,6 +16,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 
diff --git a/drivers/memstick/core/memstick.c b/drivers/memstick/core/memstick.c
index 61b98c333cb..a38005008a2 100644
--- a/drivers/memstick/core/memstick.c
+++ b/drivers/memstick/core/memstick.c
@@ -249,8 +249,11 @@ EXPORT_SYMBOL(memstick_next_req);
  */
 void memstick_new_req(struct memstick_host *host)
 {
-	host->retries = cmd_retries;
-	host->request(host);
+	if (host->card) {
+		host->retries = cmd_retries;
+		INIT_COMPLETION(host->card->mrq_complete);
+		host->request(host);
+	}
 }
 EXPORT_SYMBOL(memstick_new_req);
 
@@ -415,10 +418,14 @@ err_out:
 	return NULL;
 }
 
-static void memstick_power_on(struct memstick_host *host)
+static int memstick_power_on(struct memstick_host *host)
 {
-	host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
-	host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+	int rc = host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
+
+	if (!rc)
+		rc = host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
+
+	return rc;
 }
 
 static void memstick_check(struct work_struct *work)
@@ -429,8 +436,11 @@ static void memstick_check(struct work_struct *work)
 
 	dev_dbg(&host->dev, "memstick_check started\n");
 	mutex_lock(&host->lock);
-	if (!host->card)
-		memstick_power_on(host);
+	if (!host->card) {
+		if (memstick_power_on(host))
+			goto out_power_off;
+	} else
+		host->card->stop(host->card);
 
 	card = memstick_alloc_card(host);
 
@@ -448,7 +458,8 @@ static void memstick_check(struct work_struct *work)
 			    || !(host->card->check(host->card))) {
 				device_unregister(&host->card->dev);
 				host->card = NULL;
-			}
+			} else
+				host->card->start(host->card);
 		}
 
 		if (!host->card) {
@@ -461,6 +472,7 @@ static void memstick_check(struct work_struct *work)
 			kfree(card);
 	}
 
+out_power_off:
 	if (!host->card)
 		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
 
@@ -573,11 +585,15 @@ EXPORT_SYMBOL(memstick_suspend_host);
  */
 void memstick_resume_host(struct memstick_host *host)
 {
+	int rc = 0;
+
 	mutex_lock(&host->lock);
 	if (host->card)
-		memstick_power_on(host);
+		rc = memstick_power_on(host);
 	mutex_unlock(&host->lock);
-	memstick_detect_change(host);
+
+	if (!rc)
+		memstick_detect_change(host);
 }
 EXPORT_SYMBOL(memstick_resume_host);
 
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index 477d0fb6e58..44b1817f2f2 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -136,9 +136,8 @@ struct mspro_block_data {
 	unsigned int          caps;
 	struct gendisk        *disk;
 	struct request_queue  *queue;
+	struct request        *block_req;
 	spinlock_t            q_lock;
-	wait_queue_head_t     q_wait;
-	struct task_struct    *q_thread;
 
 	unsigned short        page_size;
 	unsigned short        cylinders;
@@ -147,9 +146,10 @@ struct mspro_block_data {
 
 	unsigned char         system;
 	unsigned char         read_only:1,
-			      active:1,
+			      eject:1,
 			      has_request:1,
-			      data_dir:1;
+			      data_dir:1,
+			      active:1;
 	unsigned char         transfer_cmd;
 
 	int                   (*mrq_handler)(struct memstick_dev *card,
@@ -160,12 +160,14 @@ struct mspro_block_data {
 	struct scatterlist    req_sg[MSPRO_BLOCK_MAX_SEGS];
 	unsigned int          seg_count;
 	unsigned int          current_seg;
-	unsigned short        current_page;
+	unsigned int          current_page;
 };
 
 static DEFINE_IDR(mspro_block_disk_idr);
 static DEFINE_MUTEX(mspro_block_disk_lock);
 
+static int mspro_block_complete_req(struct memstick_dev *card, int error);
+
 /*** Block device ***/
 
 static int mspro_block_bd_open(struct inode *inode, struct file *filp)
@@ -197,8 +199,10 @@ static int mspro_block_disk_release(struct gendisk *disk)
 
 	mutex_lock(&mspro_block_disk_lock);
 
-	if (msb->usage_count) {
-		msb->usage_count--;
+	if (msb) {
+		if (msb->usage_count)
+			msb->usage_count--;
+
 		if (!msb->usage_count) {
 			kfree(msb);
 			disk->private_data = NULL;
@@ -523,11 +527,13 @@ static int h_mspro_block_req_init(struct memstick_dev *card,
 static int h_mspro_block_default(struct memstick_dev *card,
 				 struct memstick_request **mrq)
 {
-	complete(&card->mrq_complete);
-	if (!(*mrq)->error)
-		return -EAGAIN;
-	else
-		return (*mrq)->error;
+	return mspro_block_complete_req(card, (*mrq)->error);
+}
+
+static int h_mspro_block_default_bad(struct memstick_dev *card,
+				     struct memstick_request **mrq)
+{
+	return -ENXIO;
 }
 
 static int h_mspro_block_get_ro(struct memstick_dev *card,
@@ -535,44 +541,30 @@ static int h_mspro_block_get_ro(struct memstick_dev *card,
 {
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
 
-	if ((*mrq)->error) {
-		complete(&card->mrq_complete);
-		return (*mrq)->error;
+	if (!(*mrq)->error) {
+		if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
+		    & MEMSTICK_STATUS0_WP)
+			msb->read_only = 1;
+		else
+			msb->read_only = 0;
 	}
 
-	if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
-	    & MEMSTICK_STATUS0_WP)
-		msb->read_only = 1;
-	else
-		msb->read_only = 0;
-
-	complete(&card->mrq_complete);
-	return -EAGAIN;
+	return mspro_block_complete_req(card, (*mrq)->error);
 }
 
 static int h_mspro_block_wait_for_ced(struct memstick_dev *card,
 				      struct memstick_request **mrq)
 {
-	if ((*mrq)->error) {
-		complete(&card->mrq_complete);
-		return (*mrq)->error;
-	}
-
 	dev_dbg(&card->dev, "wait for ced: value %x\n", (*mrq)->data[0]);
 
-	if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
-		card->current_mrq.error = -EFAULT;
-		complete(&card->mrq_complete);
-		return card->current_mrq.error;
+	if (!(*mrq)->error) {
+		if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR))
+			(*mrq)->error = -EFAULT;
+		else if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
+			return 0;
 	}
 
-	if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
-		return 0;
-	else {
-		card->current_mrq.error = 0;
-		complete(&card->mrq_complete);
-		return -EAGAIN;
-	}
+	return mspro_block_complete_req(card, (*mrq)->error);
 }
 
 static int h_mspro_block_transfer_data(struct memstick_dev *card,
@@ -583,10 +575,8 @@ static int h_mspro_block_transfer_data(struct memstick_dev *card,
 	struct scatterlist t_sg = { 0 };
 	size_t t_offset;
 
-	if ((*mrq)->error) {
-		complete(&card->mrq_complete);
-		return (*mrq)->error;
-	}
+	if ((*mrq)->error)
+		return mspro_block_complete_req(card, (*mrq)->error);
 
 	switch ((*mrq)->tpc) {
 	case MS_TPC_WRITE_REG:
@@ -617,8 +607,8 @@ has_int_reg:
 
 			if (msb->current_seg == msb->seg_count) {
 				if (t_val & MEMSTICK_INT_CED) {
-					complete(&card->mrq_complete);
-					return -EAGAIN;
+					return mspro_block_complete_req(card,
+									0);
 				} else {
 					card->next_request
 						= h_mspro_block_wait_for_ced;
@@ -666,140 +656,184 @@ has_int_reg:
 
 /*** Data transfer ***/
 
-static void mspro_block_process_request(struct memstick_dev *card,
-					struct request *req)
+static int mspro_block_issue_req(struct memstick_dev *card, int chunk)
 {
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
-	struct mspro_param_register param;
-	int rc, chunk, cnt;
-	unsigned short page_count;
 	sector_t t_sec;
-	unsigned long flags;
+	unsigned int count;
+	struct mspro_param_register param;
 
-	do {
-		page_count = 0;
+try_again:
+	while (chunk) {
+		msb->current_page = 0;
 		msb->current_seg = 0;
-		msb->seg_count = blk_rq_map_sg(req->q, req, msb->req_sg);
+		msb->seg_count = blk_rq_map_sg(msb->block_req->q,
+					       msb->block_req,
+					       msb->req_sg);
 
-		if (msb->seg_count) {
-			msb->current_page = 0;
-			for (rc = 0; rc < msb->seg_count; rc++)
-				page_count += msb->req_sg[rc].length
-					      / msb->page_size;
-
-			t_sec = req->sector;
-			sector_div(t_sec, msb->page_size >> 9);
-			param.system = msb->system;
-			param.data_count = cpu_to_be16(page_count);
-			param.data_address = cpu_to_be32((uint32_t)t_sec);
-			param.tpc_param = 0;
-
-			msb->data_dir = rq_data_dir(req);
-			msb->transfer_cmd = msb->data_dir == READ
-					    ? MSPRO_CMD_READ_DATA
-					    : MSPRO_CMD_WRITE_DATA;
-
-			dev_dbg(&card->dev, "data transfer: cmd %x, "
-				"lba %x, count %x\n", msb->transfer_cmd,
-				be32_to_cpu(param.data_address),
-				page_count);
-
-			card->next_request = h_mspro_block_req_init;
-			msb->mrq_handler = h_mspro_block_transfer_data;
-			memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
-					  &param, sizeof(param));
-			memstick_new_req(card->host);
-			wait_for_completion(&card->mrq_complete);
-			rc = card->current_mrq.error;
+		if (!msb->seg_count) {
+			chunk = __blk_end_request(msb->block_req, -ENOMEM,
+					blk_rq_cur_bytes(msb->block_req));
+			continue;
+		}
 
-			if (rc || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
-				for (cnt = 0; cnt < msb->current_seg; cnt++)
-					page_count += msb->req_sg[cnt].length
-						      / msb->page_size;
-
-				if (msb->current_page)
-					page_count += msb->current_page - 1;
-
-				if (page_count && (msb->data_dir == READ))
-					rc = msb->page_size * page_count;
-				else
-					rc = -EIO;
-			} else
-				rc = msb->page_size * page_count;
-		} else
-			rc = -EFAULT;
+		t_sec = msb->block_req->sector << 9;
+		sector_div(t_sec, msb->page_size);
 
-		spin_lock_irqsave(&msb->q_lock, flags);
-		if (rc >= 0)
-			chunk = __blk_end_request(req, 0, rc);
-		else
-			chunk = __blk_end_request(req, rc, 0);
+		count = msb->block_req->nr_sectors << 9;
+		count /= msb->page_size;
 
-		dev_dbg(&card->dev, "end chunk %d, %d\n", rc, chunk);
-		spin_unlock_irqrestore(&msb->q_lock, flags);
-	} while (chunk);
+		param.system = msb->system;
+		param.data_count = cpu_to_be16(count);
+		param.data_address = cpu_to_be32((uint32_t)t_sec);
+		param.tpc_param = 0;
+
+		msb->data_dir = rq_data_dir(msb->block_req);
+		msb->transfer_cmd = msb->data_dir == READ
+				    ? MSPRO_CMD_READ_DATA
+				    : MSPRO_CMD_WRITE_DATA;
+
+		dev_dbg(&card->dev, "data transfer: cmd %x, "
+			"lba %x, count %x\n", msb->transfer_cmd,
+			be32_to_cpu(param.data_address), count);
+
+		card->next_request = h_mspro_block_req_init;
+		msb->mrq_handler = h_mspro_block_transfer_data;
+		memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
+				  &param, sizeof(param));
+		memstick_new_req(card->host);
+		return 0;
+	}
+
+	dev_dbg(&card->dev, "elv_next\n");
+	msb->block_req = elv_next_request(msb->queue);
+	if (!msb->block_req) {
+		dev_dbg(&card->dev, "issue end\n");
+		return -EAGAIN;
+	}
+
+	dev_dbg(&card->dev, "trying again\n");
+	chunk = 1;
+	goto try_again;
 }
 
-static int mspro_block_has_request(struct mspro_block_data *msb)
+static int mspro_block_complete_req(struct memstick_dev *card, int error)
 {
-	int rc = 0;
+	struct mspro_block_data *msb = memstick_get_drvdata(card);
+	int chunk, cnt;
+	unsigned int t_len = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&msb->q_lock, flags);
-	if (kthread_should_stop() || msb->has_request)
-		rc = 1;
+	dev_dbg(&card->dev, "complete %d, %d\n", msb->has_request ? 1 : 0,
+		error);
+
+	if (msb->has_request) {
+		/* Nothing to do - not really an error */
+		if (error == -EAGAIN)
+			error = 0;
+
+		if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
+			if (msb->data_dir == READ) {
+				for (cnt = 0; cnt < msb->current_seg; cnt++)
+					t_len += msb->req_sg[cnt].length
+						 / msb->page_size;
+
+					if (msb->current_page)
+						t_len += msb->current_page - 1;
+
+					t_len *= msb->page_size;
+			}
+		} else
+			t_len = msb->block_req->nr_sectors << 9;
+
+		dev_dbg(&card->dev, "transferred %x (%d)\n", t_len, error);
+
+		if (error && !t_len)
+			t_len = blk_rq_cur_bytes(msb->block_req);
+
+		chunk = __blk_end_request(msb->block_req, error, t_len);
+
+		error = mspro_block_issue_req(card, chunk);
+
+		if (!error)
+			goto out;
+		else
+			msb->has_request = 0;
+	} else {
+		if (!error)
+			error = -EAGAIN;
+	}
+
+	card->next_request = h_mspro_block_default_bad;
+	complete_all(&card->mrq_complete);
+out:
 	spin_unlock_irqrestore(&msb->q_lock, flags);
-	return rc;
+	return error;
 }
 
-static int mspro_block_queue_thread(void *data)
+static void mspro_block_stop(struct memstick_dev *card)
 {
-	struct memstick_dev *card = data;
-	struct memstick_host *host = card->host;
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
-	struct request *req;
+	int rc = 0;
 	unsigned long flags;
 
 	while (1) {
-		wait_event(msb->q_wait, mspro_block_has_request(msb));
-		dev_dbg(&card->dev, "thread iter\n");
-
 		spin_lock_irqsave(&msb->q_lock, flags);
-		req = elv_next_request(msb->queue);
-		dev_dbg(&card->dev, "next req %p\n", req);
-		if (!req) {
-			msb->has_request = 0;
-			if (kthread_should_stop()) {
-				spin_unlock_irqrestore(&msb->q_lock, flags);
-				break;
-			}
-		} else
-			msb->has_request = 1;
+		if (!msb->has_request) {
+			blk_stop_queue(msb->queue);
+			rc = 1;
+		}
 		spin_unlock_irqrestore(&msb->q_lock, flags);
 
-		if (req) {
-			mutex_lock(&host->lock);
-			mspro_block_process_request(card, req);
-			mutex_unlock(&host->lock);
-		}
+		if (rc)
+			break;
+
+		wait_for_completion(&card->mrq_complete);
 	}
-	dev_dbg(&card->dev, "thread finished\n");
-	return 0;
 }
 
-static void mspro_block_request(struct request_queue *q)
+static void mspro_block_start(struct memstick_dev *card)
+{
+	struct mspro_block_data *msb = memstick_get_drvdata(card);
+	unsigned long flags;
+
+	spin_lock_irqsave(&msb->q_lock, flags);
+	blk_start_queue(msb->queue);
+	spin_unlock_irqrestore(&msb->q_lock, flags);
+}
+
+static int mspro_block_prepare_req(struct request_queue *q, struct request *req)
+{
+	if (!blk_fs_request(req) && !blk_pc_request(req)) {
+		blk_dump_rq_flags(req, "MSPro unsupported request");
+		return BLKPREP_KILL;
+	}
+
+	req->cmd_flags |= REQ_DONTPREP;
+
+	return BLKPREP_OK;
+}
+
+static void mspro_block_submit_req(struct request_queue *q)
 {
 	struct memstick_dev *card = q->queuedata;
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
 	struct request *req = NULL;
 
-	if (msb->q_thread) {
-		msb->has_request = 1;
-		wake_up_all(&msb->q_wait);
-	} else {
+	if (msb->has_request)
+		return;
+
+	if (msb->eject) {
 		while ((req = elv_next_request(q)) != NULL)
 			end_queued_request(req, -ENODEV);
+
+		return;
 	}
+
+	msb->has_request = 1;
+	if (mspro_block_issue_req(card, 0))
+		msb->has_request = 0;
 }
 
 /*** Initialization ***/
@@ -1169,16 +1203,14 @@ static int mspro_block_init_disk(struct memstick_dev *card)
 		goto out_release_id;
 	}
 
-	spin_lock_init(&msb->q_lock);
-	init_waitqueue_head(&msb->q_wait);
-
-	msb->queue = blk_init_queue(mspro_block_request, &msb->q_lock);
+	msb->queue = blk_init_queue(mspro_block_submit_req, &msb->q_lock);
 	if (!msb->queue) {
 		rc = -ENOMEM;
 		goto out_put_disk;
 	}
 
 	msb->queue->queuedata = card;
+	blk_queue_prep_rq(msb->queue, mspro_block_prepare_req);
 
 	blk_queue_bounce_limit(msb->queue, limit);
 	blk_queue_max_sectors(msb->queue, MSPRO_BLOCK_MAX_PAGES);
@@ -1204,14 +1236,8 @@ static int mspro_block_init_disk(struct memstick_dev *card)
 	capacity *= msb->page_size >> 9;
 	set_capacity(msb->disk, capacity);
 	dev_dbg(&card->dev, "capacity set %ld\n", capacity);
-	msb->q_thread = kthread_run(mspro_block_queue_thread, card,
-				    DRIVER_NAME"d");
-	if (IS_ERR(msb->q_thread))
-		goto out_put_disk;
 
-	mutex_unlock(&host->lock);
 	add_disk(msb->disk);
-	mutex_lock(&host->lock);
 	msb->active = 1;
 	return 0;
 
@@ -1259,6 +1285,7 @@ static int mspro_block_probe(struct memstick_dev *card)
 		return -ENOMEM;
 	memstick_set_drvdata(card, msb);
 	msb->card = card;
+	spin_lock_init(&msb->q_lock);
 
 	rc = mspro_block_init_card(card);
 
@@ -1272,6 +1299,8 @@ static int mspro_block_probe(struct memstick_dev *card)
 	rc = mspro_block_init_disk(card);
 	if (!rc) {
 		card->check = mspro_block_check_card;
+		card->stop = mspro_block_stop;
+		card->start = mspro_block_start;
 		return 0;
 	}
 
@@ -1286,26 +1315,17 @@ out_free:
 static void mspro_block_remove(struct memstick_dev *card)
 {
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
-	struct task_struct *q_thread = NULL;
 	unsigned long flags;
 
 	del_gendisk(msb->disk);
 	dev_dbg(&card->dev, "mspro block remove\n");
 	spin_lock_irqsave(&msb->q_lock, flags);
-	q_thread = msb->q_thread;
-	msb->q_thread = NULL;
-	msb->active = 0;
+	msb->eject = 1;
+	blk_start_queue(msb->queue);
 	spin_unlock_irqrestore(&msb->q_lock, flags);
 
-	if (q_thread) {
-		mutex_unlock(&card->host->lock);
-		kthread_stop(q_thread);
-		mutex_lock(&card->host->lock);
-	}
-
-	dev_dbg(&card->dev, "queue thread stopped\n");
-
 	blk_cleanup_queue(msb->queue);
+	msb->queue = NULL;
 
 	sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
 
@@ -1322,19 +1342,13 @@ static void mspro_block_remove(struct memstick_dev *card)
 static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state)
 {
 	struct mspro_block_data *msb = memstick_get_drvdata(card);
-	struct task_struct *q_thread = NULL;
 	unsigned long flags;
 
 	spin_lock_irqsave(&msb->q_lock, flags);
-	q_thread = msb->q_thread;
-	msb->q_thread = NULL;
-	msb->active = 0;
 	blk_stop_queue(msb->queue);
+	msb->active = 0;
 	spin_unlock_irqrestore(&msb->q_lock, flags);
 
-	if (q_thread)
-		kthread_stop(q_thread);
-
 	return 0;
 }
 
@@ -1373,14 +1387,7 @@ static int mspro_block_resume(struct memstick_dev *card)
 			if (memcmp(s_attr->data, r_attr->data, s_attr->size))
 				break;
 
-			memstick_set_drvdata(card, msb);
-			msb->q_thread = kthread_run(mspro_block_queue_thread,
-						    card, DRIVER_NAME"d");
-			if (IS_ERR(msb->q_thread))
-				msb->q_thread = NULL;
-			else
-				msb->active = 1;
-
+			msb->active = 1;
 			break;
 		}
 	}
diff --git a/drivers/memstick/host/jmb38x_ms.c b/drivers/memstick/host/jmb38x_ms.c
index 4e3bfbcdf15..3485c63d20b 100644
--- a/drivers/memstick/host/jmb38x_ms.c
+++ b/drivers/memstick/host/jmb38x_ms.c
@@ -50,6 +50,7 @@ struct jmb38x_ms_host {
 	struct jmb38x_ms        *chip;
 	void __iomem            *addr;
 	spinlock_t              lock;
+	struct tasklet_struct   notify;
 	int                     id;
 	char                    host_id[32];
 	int                     irq;
@@ -590,55 +591,97 @@ static void jmb38x_ms_abort(unsigned long data)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static void jmb38x_ms_request(struct memstick_host *msh)
+static void jmb38x_ms_req_tasklet(unsigned long data)
 {
+	struct memstick_host *msh = (struct memstick_host *)data;
 	struct jmb38x_ms_host *host = memstick_priv(msh);
 	unsigned long flags;
 	int rc;
 
 	spin_lock_irqsave(&host->lock, flags);
-	if (host->req) {
-		spin_unlock_irqrestore(&host->lock, flags);
-		BUG();
-		return;
+	if (!host->req) {
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			dev_dbg(&host->chip->pdev->dev, "tasklet req %d\n", rc);
+		} while (!rc && jmb38x_ms_issue_cmd(msh));
 	}
-
-	do {
-		rc = memstick_next_req(msh, &host->req);
-	} while (!rc && jmb38x_ms_issue_cmd(msh));
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static void jmb38x_ms_reset(struct jmb38x_ms_host *host)
+static void jmb38x_ms_dummy_submit(struct memstick_host *msh)
 {
-	unsigned int host_ctl = readl(host->addr + HOST_CONTROL);
+	return;
+}
+
+static void jmb38x_ms_submit_req(struct memstick_host *msh)
+{
+	struct jmb38x_ms_host *host = memstick_priv(msh);
+
+	tasklet_schedule(&host->notify);
+}
+
+static int jmb38x_ms_reset(struct jmb38x_ms_host *host)
+{
+	int cnt;
+
+	writel(HOST_CONTROL_RESET_REQ | HOST_CONTROL_CLOCK_EN
+	       | readl(host->addr + HOST_CONTROL),
+	       host->addr + HOST_CONTROL);
+	mmiowb();
+
+	for (cnt = 0; cnt < 20; ++cnt) {
+		if (!(HOST_CONTROL_RESET_REQ
+		      & readl(host->addr + HOST_CONTROL)))
+			goto reset_next;
 
-	writel(HOST_CONTROL_RESET_REQ, host->addr + HOST_CONTROL);
+		ndelay(20);
+	}
+	dev_dbg(&host->chip->pdev->dev, "reset_req timeout\n");
+	return -EIO;
+
+reset_next:
+	writel(HOST_CONTROL_RESET | HOST_CONTROL_CLOCK_EN
+	       | readl(host->addr + HOST_CONTROL),
+	       host->addr + HOST_CONTROL);
+	mmiowb();
+
+	for (cnt = 0; cnt < 20; ++cnt) {
+		if (!(HOST_CONTROL_RESET
+		      & readl(host->addr + HOST_CONTROL)))
+			goto reset_ok;
 
-	while (HOST_CONTROL_RESET_REQ
-	       & (host_ctl = readl(host->addr + HOST_CONTROL))) {
 		ndelay(20);
-		dev_dbg(&host->chip->pdev->dev, "reset %08x\n", host_ctl);
 	}
+	dev_dbg(&host->chip->pdev->dev, "reset timeout\n");
+	return -EIO;
 
-	writel(HOST_CONTROL_RESET, host->addr + HOST_CONTROL);
+reset_ok:
 	mmiowb();
 	writel(INT_STATUS_ALL, host->addr + INT_SIGNAL_ENABLE);
 	writel(INT_STATUS_ALL, host->addr + INT_STATUS_ENABLE);
+	return 0;
 }
 
-static void jmb38x_ms_set_param(struct memstick_host *msh,
-				enum memstick_param param,
-				int value)
+static int jmb38x_ms_set_param(struct memstick_host *msh,
+			       enum memstick_param param,
+			       int value)
 {
 	struct jmb38x_ms_host *host = memstick_priv(msh);
 	unsigned int host_ctl = readl(host->addr + HOST_CONTROL);
 	unsigned int clock_ctl = CLOCK_CONTROL_40MHZ, clock_delay = 0;
+	int rc = 0;
 
 	switch (param) {
 	case MEMSTICK_POWER:
 		if (value == MEMSTICK_POWER_ON) {
-			jmb38x_ms_reset(host);
+			rc = jmb38x_ms_reset(host);
+			if (rc)
+				return rc;
+
+			host_ctl = 7;
+			host_ctl |= HOST_CONTROL_POWER_EN
+				 | HOST_CONTROL_CLOCK_EN;
+			writel(host_ctl, host->addr + HOST_CONTROL);
 
 			writel(host->id ? PAD_PU_PD_ON_MS_SOCK1
 					: PAD_PU_PD_ON_MS_SOCK0,
@@ -647,11 +690,7 @@ static void jmb38x_ms_set_param(struct memstick_host *msh,
 			writel(PAD_OUTPUT_ENABLE_MS,
 			       host->addr + PAD_OUTPUT_ENABLE);
 
-			host_ctl = 7;
-			host_ctl |= HOST_CONTROL_POWER_EN
-				 | HOST_CONTROL_CLOCK_EN;
-			writel(host_ctl, host->addr + HOST_CONTROL);
-
+			msleep(10);
 			dev_dbg(&host->chip->pdev->dev, "power on\n");
 		} else if (value == MEMSTICK_POWER_OFF) {
 			host_ctl &= ~(HOST_CONTROL_POWER_EN
@@ -660,7 +699,8 @@ static void jmb38x_ms_set_param(struct memstick_host *msh,
 			writel(0, host->addr + PAD_OUTPUT_ENABLE);
 			writel(PAD_PU_PD_OFF, host->addr + PAD_PU_PD);
 			dev_dbg(&host->chip->pdev->dev, "power off\n");
-		}
+		} else
+			return -EINVAL;
 		break;
 	case MEMSTICK_INTERFACE:
 		host_ctl &= ~(3 << HOST_CONTROL_IF_SHIFT);
@@ -686,12 +726,14 @@ static void jmb38x_ms_set_param(struct memstick_host *msh,
 			host_ctl &= ~HOST_CONTROL_REI;
 			clock_ctl = CLOCK_CONTROL_60MHZ;
 			clock_delay = 0;
-		}
+		} else
+			return -EINVAL;
 		writel(host_ctl, host->addr + HOST_CONTROL);
 		writel(clock_ctl, host->addr + CLOCK_CONTROL);
 		writel(clock_delay, host->addr + CLOCK_DELAY);
 		break;
 	};
+	return 0;
 }
 
 #ifdef CONFIG_PM
@@ -785,7 +827,9 @@ static struct memstick_host *jmb38x_ms_alloc_host(struct jmb38x_ms *jm, int cnt)
 		 host->id);
 	host->irq = jm->pdev->irq;
 	host->timeout_jiffies = msecs_to_jiffies(1000);
-	msh->request = jmb38x_ms_request;
+
+	tasklet_init(&host->notify, jmb38x_ms_req_tasklet, (unsigned long)msh);
+	msh->request = jmb38x_ms_submit_req;
 	msh->set_param = jmb38x_ms_set_param;
 
 	msh->caps = MEMSTICK_CAP_PAR4 | MEMSTICK_CAP_PAR8;
@@ -897,6 +941,8 @@ static void jmb38x_ms_remove(struct pci_dev *dev)
 
 		host = memstick_priv(jm->hosts[cnt]);
 
+		jm->hosts[cnt]->request = jmb38x_ms_dummy_submit;
+		tasklet_kill(&host->notify);
 		writel(0, host->addr + INT_SIGNAL_ENABLE);
 		writel(0, host->addr + INT_STATUS_ENABLE);
 		mmiowb();
diff --git a/drivers/memstick/host/tifm_ms.c b/drivers/memstick/host/tifm_ms.c
index 8577de4ebb0..d32d6ad8f3f 100644
--- a/drivers/memstick/host/tifm_ms.c
+++ b/drivers/memstick/host/tifm_ms.c
@@ -71,6 +71,7 @@ struct tifm_ms {
 	struct tifm_dev         *dev;
 	struct timer_list       timer;
 	struct memstick_request *req;
+	struct tasklet_struct   notify;
 	unsigned int            mode_mask;
 	unsigned int            block_pos;
 	unsigned long           timeout_jiffies;
@@ -455,49 +456,51 @@ static void tifm_ms_card_event(struct tifm_dev *sock)
 	return;
 }
 
-static void tifm_ms_request(struct memstick_host *msh)
+static void tifm_ms_req_tasklet(unsigned long data)
 {
+	struct memstick_host *msh = (struct memstick_host *)data;
 	struct tifm_ms *host = memstick_priv(msh);
 	struct tifm_dev *sock = host->dev;
 	unsigned long flags;
 	int rc;
 
 	spin_lock_irqsave(&sock->lock, flags);
-	if (host->req) {
-		printk(KERN_ERR "%s : unfinished request detected\n",
-		       sock->dev.bus_id);
-		spin_unlock_irqrestore(&sock->lock, flags);
-		tifm_eject(host->dev);
-		return;
-	}
+	if (!host->req) {
+		if (host->eject) {
+			do {
+				rc = memstick_next_req(msh, &host->req);
+				if (!rc)
+					host->req->error = -ETIME;
+			} while (!rc);
+			spin_unlock_irqrestore(&sock->lock, flags);
+			return;
+		}
 
-	if (host->eject) {
 		do {
 			rc = memstick_next_req(msh, &host->req);
-			if (!rc)
-				host->req->error = -ETIME;
-		} while (!rc);
-		spin_unlock_irqrestore(&sock->lock, flags);
-		return;
+		} while (!rc && tifm_ms_issue_cmd(host));
 	}
-
-	do {
-		rc = memstick_next_req(msh, &host->req);
-	} while (!rc && tifm_ms_issue_cmd(host));
-
 	spin_unlock_irqrestore(&sock->lock, flags);
+}
+
+static void tifm_ms_dummy_submit(struct memstick_host *msh)
+{
 	return;
 }
 
-static void tifm_ms_set_param(struct memstick_host *msh,
-			      enum memstick_param param,
-			      int value)
+static void tifm_ms_submit_req(struct memstick_host *msh)
 {
 	struct tifm_ms *host = memstick_priv(msh);
-	struct tifm_dev *sock = host->dev;
-	unsigned long flags;
 
-	spin_lock_irqsave(&sock->lock, flags);
+	tasklet_schedule(&host->notify);
+}
+
+static int tifm_ms_set_param(struct memstick_host *msh,
+			     enum memstick_param param,
+			     int value)
+{
+	struct tifm_ms *host = memstick_priv(msh);
+	struct tifm_dev *sock = host->dev;
 
 	switch (param) {
 	case MEMSTICK_POWER:
@@ -512,7 +515,8 @@ static void tifm_ms_set_param(struct memstick_host *msh,
 			writel(TIFM_MS_SYS_FCLR | TIFM_MS_SYS_INTCLR,
 			       sock->addr + SOCK_MS_SYSTEM);
 			writel(0xffffffff, sock->addr + SOCK_MS_STATUS);
-		}
+		} else
+			return -EINVAL;
 		break;
 	case MEMSTICK_INTERFACE:
 		if (value == MEMSTICK_SERIAL) {
@@ -525,11 +529,12 @@ static void tifm_ms_set_param(struct memstick_host *msh,
 			writel(TIFM_CTRL_FAST_CLK
 			       | readl(sock->addr + SOCK_CONTROL),
 			       sock->addr + SOCK_CONTROL);
-		}
+		} else
+			return -EINVAL;
 		break;
 	};
 
-	spin_unlock_irqrestore(&sock->lock, flags);
+	return 0;
 }
 
 static void tifm_ms_abort(unsigned long data)
@@ -570,8 +575,9 @@ static int tifm_ms_probe(struct tifm_dev *sock)
 	host->timeout_jiffies = msecs_to_jiffies(1000);
 
 	setup_timer(&host->timer, tifm_ms_abort, (unsigned long)host);
+	tasklet_init(&host->notify, tifm_ms_req_tasklet, (unsigned long)msh);
 
-	msh->request = tifm_ms_request;
+	msh->request = tifm_ms_submit_req;
 	msh->set_param = tifm_ms_set_param;
 	sock->card_event = tifm_ms_card_event;
 	sock->data_event = tifm_ms_data_event;
@@ -593,6 +599,8 @@ static void tifm_ms_remove(struct tifm_dev *sock)
 	int rc = 0;
 	unsigned long flags;
 
+	msh->request = tifm_ms_dummy_submit;
+	tasklet_kill(&host->notify);
 	spin_lock_irqsave(&sock->lock, flags);
 	host->eject = 1;
 	if (host->req) {
diff --git a/drivers/message/fusion/lsi/mpi_history.txt b/drivers/message/fusion/lsi/mpi_history.txt
index 241592ab13a..3f15fcfe4a2 100644
--- a/drivers/message/fusion/lsi/mpi_history.txt
+++ b/drivers/message/fusion/lsi/mpi_history.txt
@@ -127,7 +127,7 @@ mpi_ioc.h
  *  08-08-01  01.02.01  Original release for v1.2 work.
  *                      New format for FWVersion and ProductId in
  *                      MSG_IOC_FACTS_REPLY and MPI_FW_HEADER.
- *  08-31-01  01.02.02  Addded event MPI_EVENT_SCSI_DEVICE_STATUS_CHANGE and
+ *  08-31-01  01.02.02  Added event MPI_EVENT_SCSI_DEVICE_STATUS_CHANGE and
  *                      related structure and defines.
  *                      Added event MPI_EVENT_ON_BUS_TIMER_EXPIRED.
  *                      Added MPI_IOCINIT_FLAGS_DISCARD_FW_IMAGE.
@@ -187,7 +187,7 @@ mpi_ioc.h
  *  10-11-06  01.05.12  Added MPI_IOCFACTS_EXCEPT_METADATA_UNSUPPORTED.
  *                      Added MaxInitiators field to PortFacts reply.
  *                      Added SAS Device Status Change ReasonCode for
- *                      asynchronous notificaiton.
+ *                      asynchronous notification.
  *                      Added MPI_EVENT_SAS_EXPANDER_STATUS_CHANGE and event
  *                      data structure.
  *                      Added new ImageType values for FWDownload and FWUpload
@@ -213,7 +213,7 @@ mpi_cnfg.h
  *                      Added _RESPONSE_ID_MASK definition to SCSI_PORT_1
  *                      page and updated the page version.
  *                      Added Information field and _INFO_PARAMS_NEGOTIATED
- *                      definitionto SCSI_DEVICE_0 page.
+ *                      definition to SCSI_DEVICE_0 page.
  *  06-22-00  01.00.03  Removed batch controls from LAN_0 page and updated the
  *                      page version.
  *                      Added BucketsRemaining to LAN_1 page, redefined the
diff --git a/drivers/message/i2o/device.c b/drivers/message/i2o/device.c
index 489d7c5c496..8774c670e66 100644
--- a/drivers/message/i2o/device.c
+++ b/drivers/message/i2o/device.c
@@ -243,29 +243,41 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	/* create user entries for this device */
 	tmp = i2o_iop_find_device(i2o_dev->iop, i2o_dev->lct_data.user_tid);
-	if (tmp && (tmp != i2o_dev))
-		sysfs_create_link(&i2o_dev->device.kobj, &tmp->device.kobj,
-				  "user");
+	if (tmp && (tmp != i2o_dev)) {
+		rc = sysfs_create_link(&i2o_dev->device.kobj,
+				       &tmp->device.kobj, "user");
+		if (rc)
+			goto unreg_dev;
+	}
 
 	/* create user entries refering to this device */
 	list_for_each_entry(tmp, &c->devices, list)
 	    if ((tmp->lct_data.user_tid == i2o_dev->lct_data.tid)
-		&& (tmp != i2o_dev))
-		sysfs_create_link(&tmp->device.kobj,
-				  &i2o_dev->device.kobj, "user");
+		&& (tmp != i2o_dev)) {
+		rc = sysfs_create_link(&tmp->device.kobj,
+				       &i2o_dev->device.kobj, "user");
+		if (rc)
+			goto rmlink1;
+	}
 
 	/* create parent entries for this device */
 	tmp = i2o_iop_find_device(i2o_dev->iop, i2o_dev->lct_data.parent_tid);
-	if (tmp && (tmp != i2o_dev))
-		sysfs_create_link(&i2o_dev->device.kobj, &tmp->device.kobj,
-				  "parent");
+	if (tmp && (tmp != i2o_dev)) {
+		rc = sysfs_create_link(&i2o_dev->device.kobj,
+				       &tmp->device.kobj, "parent");
+		if (rc)
+			goto rmlink1;
+	}
 
 	/* create parent entries refering to this device */
 	list_for_each_entry(tmp, &c->devices, list)
 	    if ((tmp->lct_data.parent_tid == i2o_dev->lct_data.tid)
-		&& (tmp != i2o_dev))
-		sysfs_create_link(&tmp->device.kobj,
-				  &i2o_dev->device.kobj, "parent");
+		&& (tmp != i2o_dev)) {
+		rc = sysfs_create_link(&tmp->device.kobj,
+				       &i2o_dev->device.kobj, "parent");
+		if (rc)
+			goto rmlink2;
+	}
 
 	i2o_driver_notify_device_add_all(i2o_dev);
 
@@ -273,6 +285,24 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	return 0;
 
+rmlink2:
+	/* If link creating failed halfway, we loop whole list to cleanup.
+	 * And we don't care wrong removing of link, because sysfs_remove_link
+	 * will take care of it.
+	 */
+	list_for_each_entry(tmp, &c->devices, list) {
+		if (tmp->lct_data.parent_tid == i2o_dev->lct_data.tid)
+			sysfs_remove_link(&tmp->device.kobj, "parent");
+	}
+	sysfs_remove_link(&i2o_dev->device.kobj, "parent");
+rmlink1:
+	list_for_each_entry(tmp, &c->devices, list)
+		if (tmp->lct_data.user_tid == i2o_dev->lct_data.tid)
+			sysfs_remove_link(&tmp->device.kobj, "user");
+	sysfs_remove_link(&i2o_dev->device.kobj, "user");
+unreg_dev:
+	list_del(&i2o_dev->list);
+	device_unregister(&i2o_dev->device);
 err:
 	kfree(i2o_dev);
 	return rc;
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 9f93c29fed3..883e7ea31de 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -19,6 +19,14 @@ config MFD_SM501
 	  interface. The device may be connected by PCI or local bus with
 	  varying functions enabled.
 
+config MFD_SM501_GPIO
+	bool "Export GPIO via GPIO layer"
+	depends on MFD_SM501 && HAVE_GPIO_LIB
+	 ---help---
+	 This option uses the gpio library layer to export the 64 GPIO
+	 lines on the SM501. The platform data is used to supply the
+	 base number for the first GPIO line to register.
+
 config MFD_ASIC3
 	bool "Support for Compaq ASIC3"
 	depends on GENERIC_HARDIRQS && HAVE_GPIO_LIB && ARM
@@ -28,7 +36,7 @@ config MFD_ASIC3
 
 config HTC_EGPIO
 	bool "HTC EGPIO support"
-	depends on GENERIC_HARDIRQS && HAVE_GPIO_LIB && ARM
+	depends on GENERIC_HARDIRQS && GPIOLIB && ARM
 	help
 	    This driver supports the CPLD egpio chip present on
 	    several HTC phones.  It provides basic support for input
@@ -44,7 +52,7 @@ config HTC_PASIC3
 
 config MFD_TC6393XB
 	bool "Support Toshiba TC6393XB"
-	depends on HAVE_GPIO_LIB
+	depends on GPIOLIB && ARM
 	select MFD_CORE
 	help
 	  Support for Toshiba Mobile IO Controller TC6393XB
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index 3b870e7fb3e..eabf0bfccab 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -314,10 +314,12 @@ static int __init asic3_irq_probe(struct platform_device *pdev)
 	unsigned long clksel = 0;
 	unsigned int irq, irq_base;
 	int map_size;
+	int ret;
 
-	asic->irq_nr = platform_get_irq(pdev, 0);
-	if (asic->irq_nr < 0)
-		return asic->irq_nr;
+	ret = platform_get_irq(pdev, 0);
+	if (ret < 0)
+		return ret;
+	asic->irq_nr = ret;
 
 	/* turn on clock to IRQ controller */
 	clksel |= CLOCK_SEL_CX;
diff --git a/drivers/mfd/htc-egpio.c b/drivers/mfd/htc-egpio.c
index 8872cc07751..6be43172dc6 100644
--- a/drivers/mfd/htc-egpio.c
+++ b/drivers/mfd/htc-egpio.c
@@ -318,6 +318,8 @@ static int __init egpio_probe(struct platform_device *pdev)
 		ei->chip[i].dev = &(pdev->dev);
 		chip = &(ei->chip[i].chip);
 		chip->label           = "htc-egpio";
+		chip->dev             = &pdev->dev;
+		chip->owner           = THIS_MODULE;
 		chip->get             = egpio_get;
 		chip->set             = egpio_set;
 		chip->direction_input = egpio_direction_input;
diff --git a/drivers/mfd/htc-pasic3.c b/drivers/mfd/htc-pasic3.c
index 633cbba072f..91b294dcc13 100644
--- a/drivers/mfd/htc-pasic3.c
+++ b/drivers/mfd/htc-pasic3.c
@@ -238,6 +238,8 @@ static int pasic3_remove(struct platform_device *pdev)
 	return 0;
 }
 
+MODULE_ALIAS("platform:pasic3");
+
 static struct platform_driver pasic3_driver = {
 	.driver		= {
 		.name	= "pasic3",
diff --git a/drivers/mfd/mcp-sa11x0.c b/drivers/mfd/mcp-sa11x0.c
index 1eab7cffcea..b5272b5ce3f 100644
--- a/drivers/mfd/mcp-sa11x0.c
+++ b/drivers/mfd/mcp-sa11x0.c
@@ -242,6 +242,8 @@ static int mcp_sa11x0_resume(struct platform_device *dev)
 /*
  * The driver for the SA11x0 MCP port.
  */
+MODULE_ALIAS("platform:sa11x0-mcp");
+
 static struct platform_driver mcp_sa11x0_driver = {
 	.probe		= mcp_sa11x0_probe,
 	.remove		= mcp_sa11x0_remove,
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index d7d88ce053a..0454be4266c 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -36,7 +36,7 @@ static int mfd_add_device(struct platform_device *parent,
 	if (ret)
 		goto fail_device;
 
-	memzero(res, sizeof(res));
+	memset(res, 0, sizeof(res));
 	for (r = 0; r < cell->num_resources; r++) {
 		res[r].name = cell->resources[r].name;
 		res[r].flags = cell->resources[r].flags;
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 2fe64734d8a..7aebad4c06f 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -19,6 +19,7 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/pci.h>
+#include <linux/i2c-gpio.h>
 
 #include <linux/sm501.h>
 #include <linux/sm501-regs.h>
@@ -31,10 +32,37 @@ struct sm501_device {
 	struct platform_device		pdev;
 };
 
+struct sm501_gpio;
+
+#ifdef CONFIG_MFD_SM501_GPIO
+#include <linux/gpio.h>
+
+struct sm501_gpio_chip {
+	struct gpio_chip	gpio;
+	struct sm501_gpio	*ourgpio;	/* to get back to parent. */
+	void __iomem		*regbase;
+};
+
+struct sm501_gpio {
+	struct sm501_gpio_chip	low;
+	struct sm501_gpio_chip	high;
+	spinlock_t		lock;
+
+	unsigned int		 registered : 1;
+	void __iomem		*regs;
+	struct resource		*regs_res;
+};
+#else
+struct sm501_gpio {
+	/* no gpio support, empty definition for sm501_devdata. */
+};
+#endif
+
 struct sm501_devdata {
 	spinlock_t			 reg_lock;
 	struct mutex			 clock_lock;
 	struct list_head		 devices;
+	struct sm501_gpio		 gpio;
 
 	struct device			*dev;
 	struct resource			*io_res;
@@ -42,6 +70,7 @@ struct sm501_devdata {
 	struct resource			*regs_claim;
 	struct sm501_platdata		*platdata;
 
+
 	unsigned int			 in_suspend;
 	unsigned long			 pm_misc;
 
@@ -52,6 +81,7 @@ struct sm501_devdata {
 	unsigned int			 rev;
 };
 
+
 #define MHZ (1000 * 1000)
 
 #ifdef DEBUG
@@ -276,58 +306,6 @@ unsigned long sm501_modify_reg(struct device *dev,
 
 EXPORT_SYMBOL_GPL(sm501_modify_reg);
 
-unsigned long sm501_gpio_get(struct device *dev,
-			     unsigned long gpio)
-{
-	struct sm501_devdata *sm = dev_get_drvdata(dev);
-	unsigned long result;
-	unsigned long reg;
-
-	reg = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW;
-	result = readl(sm->regs + reg);
-
-	result >>= (gpio & 31);
-	return result & 1UL;
-}
-
-EXPORT_SYMBOL_GPL(sm501_gpio_get);
-
-void sm501_gpio_set(struct device *dev,
-		    unsigned long gpio,
-		    unsigned int to,
-		    unsigned int dir)
-{
-	struct sm501_devdata *sm = dev_get_drvdata(dev);
-
-	unsigned long bit = 1 << (gpio & 31);
-	unsigned long base;
-	unsigned long save;
-	unsigned long val;
-
-	base = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW;
-	base += SM501_GPIO;
-
-	spin_lock_irqsave(&sm->reg_lock, save);
-
-	val = readl(sm->regs + base) & ~bit;
-	if (to)
-		val |= bit;
-	writel(val, sm->regs + base);
-
-	val = readl(sm->regs + SM501_GPIO_DDR_LOW) & ~bit;
-	if (dir)
-		val |= bit;
-
-	writel(val, sm->regs + SM501_GPIO_DDR_LOW);
-	sm501_sync_regs(sm);
-
-	spin_unlock_irqrestore(&sm->reg_lock, save);
-
-}
-
-EXPORT_SYMBOL_GPL(sm501_gpio_set);
-
-
 /* sm501_unit_power
  *
  * alters the power active gate to set specific units on or off
@@ -906,6 +884,313 @@ static int sm501_register_display(struct sm501_devdata *sm,
 	return sm501_register_device(sm, pdev);
 }
 
+#ifdef CONFIG_MFD_SM501_GPIO
+
+static inline struct sm501_gpio_chip *to_sm501_gpio(struct gpio_chip *gc)
+{
+	return container_of(gc, struct sm501_gpio_chip, gpio);
+}
+
+static inline struct sm501_devdata *sm501_gpio_to_dev(struct sm501_gpio *gpio)
+{
+	return container_of(gpio, struct sm501_devdata, gpio);
+}
+
+static int sm501_gpio_get(struct gpio_chip *chip, unsigned offset)
+
+{
+	struct sm501_gpio_chip *smgpio = to_sm501_gpio(chip);
+	unsigned long result;
+
+	result = readl(smgpio->regbase + SM501_GPIO_DATA_LOW);
+	result >>= offset;
+
+	return result & 1UL;
+}
+
+static void sm501_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+
+{
+	struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+	struct sm501_gpio *smgpio = smchip->ourgpio;
+	unsigned long bit = 1 << offset;
+	void __iomem *regs = smchip->regbase;
+	unsigned long save;
+	unsigned long val;
+
+	dev_dbg(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
+		__func__, chip, offset);
+
+	spin_lock_irqsave(&smgpio->lock, save);
+
+	val = readl(regs + SM501_GPIO_DATA_LOW) & ~bit;
+	if (value)
+		val |= bit;
+	writel(val, regs);
+
+	sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+	spin_unlock_irqrestore(&smgpio->lock, save);
+}
+
+static int sm501_gpio_input(struct gpio_chip *chip, unsigned offset)
+{
+	struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+	struct sm501_gpio *smgpio = smchip->ourgpio;
+	void __iomem *regs = smchip->regbase;
+	unsigned long bit = 1 << offset;
+	unsigned long save;
+	unsigned long ddr;
+
+	dev_info(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
+		 __func__, chip, offset);
+
+	spin_lock_irqsave(&smgpio->lock, save);
+
+	ddr = readl(regs + SM501_GPIO_DDR_LOW);
+	writel(ddr & ~bit, regs + SM501_GPIO_DDR_LOW);
+
+	sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+	spin_unlock_irqrestore(&smgpio->lock, save);
+
+	return 0;
+}
+
+static int sm501_gpio_output(struct gpio_chip *chip,
+			     unsigned offset, int value)
+{
+	struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+	struct sm501_gpio *smgpio = smchip->ourgpio;
+	unsigned long bit = 1 << offset;
+	void __iomem *regs = smchip->regbase;
+	unsigned long save;
+	unsigned long val;
+	unsigned long ddr;
+
+	dev_dbg(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d,%d)\n",
+		__func__, chip, offset, value);
+
+	spin_lock_irqsave(&smgpio->lock, save);
+
+	val = readl(regs + SM501_GPIO_DATA_LOW);
+	if (value)
+		val |= bit;
+	else
+		val &= ~bit;
+	writel(val, regs);
+
+	ddr = readl(regs + SM501_GPIO_DDR_LOW);
+	writel(ddr | bit, regs + SM501_GPIO_DDR_LOW);
+
+	sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+	writel(val, regs + SM501_GPIO_DATA_LOW);
+
+	sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+	spin_unlock_irqrestore(&smgpio->lock, save);
+
+	return 0;
+}
+
+static struct gpio_chip gpio_chip_template = {
+	.ngpio			= 32,
+	.direction_input	= sm501_gpio_input,
+	.direction_output	= sm501_gpio_output,
+	.set			= sm501_gpio_set,
+	.get			= sm501_gpio_get,
+};
+
+static int __devinit sm501_gpio_register_chip(struct sm501_devdata *sm,
+					      struct sm501_gpio *gpio,
+					      struct sm501_gpio_chip *chip)
+{
+	struct sm501_platdata *pdata = sm->platdata;
+	struct gpio_chip *gchip = &chip->gpio;
+	int base = pdata->gpio_base;
+
+	chip->gpio = gpio_chip_template;
+
+	if (chip == &gpio->high) {
+		if (base > 0)
+			base += 32;
+		chip->regbase = gpio->regs + SM501_GPIO_DATA_HIGH;
+		gchip->label  = "SM501-HIGH";
+	} else {
+		chip->regbase = gpio->regs + SM501_GPIO_DATA_LOW;
+		gchip->label  = "SM501-LOW";
+	}
+
+	gchip->base   = base;
+	chip->ourgpio = gpio;
+
+	return gpiochip_add(gchip);
+}
+
+static int sm501_register_gpio(struct sm501_devdata *sm)
+{
+	struct sm501_gpio *gpio = &sm->gpio;
+	resource_size_t iobase = sm->io_res->start + SM501_GPIO;
+	int ret;
+	int tmp;
+
+	dev_dbg(sm->dev, "registering gpio block %08llx\n",
+		(unsigned long long)iobase);
+
+	spin_lock_init(&gpio->lock);
+
+	gpio->regs_res = request_mem_region(iobase, 0x20, "sm501-gpio");
+	if (gpio->regs_res == NULL) {
+		dev_err(sm->dev, "gpio: failed to request region\n");
+		return -ENXIO;
+	}
+
+	gpio->regs = ioremap(iobase, 0x20);
+	if (gpio->regs == NULL) {
+		dev_err(sm->dev, "gpio: failed to remap registers\n");
+		ret = -ENXIO;
+		goto err_claimed;
+	}
+
+	/* Register both our chips. */
+
+	ret = sm501_gpio_register_chip(sm, gpio, &gpio->low);
+	if (ret) {
+		dev_err(sm->dev, "failed to add low chip\n");
+		goto err_mapped;
+	}
+
+	ret = sm501_gpio_register_chip(sm, gpio, &gpio->high);
+	if (ret) {
+		dev_err(sm->dev, "failed to add high chip\n");
+		goto err_low_chip;
+	}
+
+	gpio->registered = 1;
+
+	return 0;
+
+ err_low_chip:
+	tmp = gpiochip_remove(&gpio->low.gpio);
+	if (tmp) {
+		dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
+		return ret;
+	}
+
+ err_mapped:
+	iounmap(gpio->regs);
+
+ err_claimed:
+	release_resource(gpio->regs_res);
+	kfree(gpio->regs_res);
+
+	return ret;
+}
+
+static void sm501_gpio_remove(struct sm501_devdata *sm)
+{
+	struct sm501_gpio *gpio = &sm->gpio;
+	int ret;
+
+	if (!sm->gpio.registered)
+		return;
+
+	ret = gpiochip_remove(&gpio->low.gpio);
+	if (ret)
+		dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
+
+	ret = gpiochip_remove(&gpio->high.gpio);
+	if (ret)
+		dev_err(sm->dev, "cannot remove high chip, cannot tidy up\n");
+
+	iounmap(gpio->regs);
+	release_resource(gpio->regs_res);
+	kfree(gpio->regs_res);
+}
+
+static inline int sm501_gpio_pin2nr(struct sm501_devdata *sm, unsigned int pin)
+{
+	struct sm501_gpio *gpio = &sm->gpio;
+	int base = (pin < 32) ? gpio->low.gpio.base : gpio->high.gpio.base;
+
+	return (pin % 32) + base;
+}
+
+static inline int sm501_gpio_isregistered(struct sm501_devdata *sm)
+{
+	return sm->gpio.registered;
+}
+#else
+static inline int sm501_register_gpio(struct sm501_devdata *sm)
+{
+	return 0;
+}
+
+static inline void sm501_gpio_remove(struct sm501_devdata *sm)
+{
+}
+
+static inline int sm501_gpio_pin2nr(struct sm501_devdata *sm, unsigned int pin)
+{
+	return -1;
+}
+
+static inline int sm501_gpio_isregistered(struct sm501_devdata *sm)
+{
+	return 0;
+}
+#endif
+
+static int sm501_register_gpio_i2c_instance(struct sm501_devdata *sm,
+					    struct sm501_platdata_gpio_i2c *iic)
+{
+	struct i2c_gpio_platform_data *icd;
+	struct platform_device *pdev;
+
+	pdev = sm501_create_subdev(sm, "i2c-gpio", 0,
+				   sizeof(struct i2c_gpio_platform_data));
+	if (!pdev)
+		return -ENOMEM;
+
+	icd = pdev->dev.platform_data;
+
+	/* We keep the pin_sda and pin_scl fields relative in case the
+	 * same platform data is passed to >1 SM501.
+	 */
+
+	icd->sda_pin = sm501_gpio_pin2nr(sm, iic->pin_sda);
+	icd->scl_pin = sm501_gpio_pin2nr(sm, iic->pin_scl);
+	icd->timeout = iic->timeout;
+	icd->udelay = iic->udelay;
+
+	/* note, we can't use either of the pin numbers, as the i2c-gpio
+	 * driver uses the platform.id field to generate the bus number
+	 * to register with the i2c core; The i2c core doesn't have enough
+	 * entries to deal with anything we currently use.
+	*/
+
+	pdev->id = iic->bus_num;
+
+	dev_info(sm->dev, "registering i2c-%d: sda=%d (%d), scl=%d (%d)\n",
+		 iic->bus_num,
+		 icd->sda_pin, iic->pin_sda, icd->scl_pin, iic->pin_scl);
+
+	return sm501_register_device(sm, pdev);
+}
+
+static int sm501_register_gpio_i2c(struct sm501_devdata *sm,
+				   struct sm501_platdata *pdata)
+{
+	struct sm501_platdata_gpio_i2c *iic = pdata->gpio_i2c;
+	int index;
+	int ret;
+
+	for (index = 0; index < pdata->gpio_i2c_nr; index++, iic++) {
+		ret = sm501_register_gpio_i2c_instance(sm, iic);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 /* sm501_dbg_regs
  *
  * Debug attribute to attach to parent device to show core registers
@@ -1013,6 +1298,7 @@ static unsigned int sm501_mem_local[] = {
 static int sm501_init_dev(struct sm501_devdata *sm)
 {
 	struct sm501_initdata *idata;
+	struct sm501_platdata *pdata;
 	resource_size_t mem_avail;
 	unsigned long dramctrl;
 	unsigned long devid;
@@ -1051,7 +1337,9 @@ static int sm501_init_dev(struct sm501_devdata *sm)
 
 	/* check to see if we have some device initialisation */
 
-	idata = sm->platdata ? sm->platdata->init : NULL;
+	pdata = sm->platdata;
+	idata = pdata ? pdata->init : NULL;
+
 	if (idata) {
 		sm501_init_regs(sm, idata);
 
@@ -1059,6 +1347,15 @@ static int sm501_init_dev(struct sm501_devdata *sm)
 			sm501_register_usbhost(sm, &mem_avail);
 		if (idata->devices & (SM501_USE_UART0 | SM501_USE_UART1))
 			sm501_register_uart(sm, idata->devices);
+		if (idata->devices & SM501_USE_GPIO)
+			sm501_register_gpio(sm);
+	}
+
+	if (pdata->gpio_i2c != NULL && pdata->gpio_i2c_nr > 0) {
+		if (!sm501_gpio_isregistered(sm))
+			dev_err(sm->dev, "no gpio available for i2c gpio.\n");
+		else
+			sm501_register_gpio_i2c(sm, pdata);
 	}
 
 	ret = sm501_check_clocks(sm);
@@ -1138,8 +1435,31 @@ static int sm501_plat_probe(struct platform_device *dev)
 }
 
 #ifdef CONFIG_PM
+
 /* power management support */
 
+static void sm501_set_power(struct sm501_devdata *sm, int on)
+{
+	struct sm501_platdata *pd = sm->platdata;
+
+	if (pd == NULL)
+		return;
+
+	if (pd->get_power) {
+		if (pd->get_power(sm->dev) == on) {
+			dev_dbg(sm->dev, "is already %d\n", on);
+			return;
+		}
+	}
+
+	if (pd->set_power) {
+		dev_dbg(sm->dev, "setting power to %d\n", on);
+
+		pd->set_power(sm->dev, on);
+		sm501_mdelay(sm, 10);
+	}
+}
+
 static int sm501_plat_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct sm501_devdata *sm = platform_get_drvdata(pdev);
@@ -1148,6 +1468,12 @@ static int sm501_plat_suspend(struct platform_device *pdev, pm_message_t state)
 	sm->pm_misc = readl(sm->regs + SM501_MISC_CONTROL);
 
 	sm501_dump_regs(sm);
+
+	if (sm->platdata) {
+		if (sm->platdata->flags & SM501_FLAG_SUSPEND_OFF)
+			sm501_set_power(sm, 0);
+	}
+
 	return 0;
 }
 
@@ -1155,6 +1481,8 @@ static int sm501_plat_resume(struct platform_device *pdev)
 {
 	struct sm501_devdata *sm = platform_get_drvdata(pdev);
 
+	sm501_set_power(sm, 1);
+
 	sm501_dump_regs(sm);
 	sm501_dump_gate(sm);
 	sm501_dump_clk(sm);
@@ -1229,6 +1557,7 @@ static struct sm501_platdata_fb sm501_fb_pdata = {
 static struct sm501_platdata sm501_pci_platdata = {
 	.init		= &sm501_pci_initdata,
 	.fb		= &sm501_fb_pdata,
+	.gpio_base	= -1,
 };
 
 static int sm501_pci_probe(struct pci_dev *dev,
@@ -1335,6 +1664,8 @@ static void sm501_dev_remove(struct sm501_devdata *sm)
 		sm501_remove_sub(sm, smdev);
 
 	device_remove_file(sm->dev, &dev_attr_dbg_regs);
+
+	sm501_gpio_remove(sm);
 }
 
 static void sm501_pci_remove(struct pci_dev *dev)
@@ -1378,6 +1709,8 @@ static struct pci_driver sm501_pci_drv = {
 	.remove		= sm501_pci_remove,
 };
 
+MODULE_ALIAS("platform:sm501");
+
 static struct platform_driver sm501_plat_drv = {
 	.driver		= {
 		.name	= "sm501",
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index ce67d973d34..321eb913463 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -77,11 +77,13 @@ config IBM_ASM
 	  for your IBM server.
 
 config PHANTOM
-	tristate "Sensable PHANToM"
+	tristate "Sensable PHANToM (PCI)"
 	depends on PCI
 	help
 	  Say Y here if you want to build a driver for Sensable PHANToM device.
 
+	  This driver is only for PCI PHANToMs.
+
 	  If you choose to build module, its name will be phantom. If unsure,
 	  say N here.
 
@@ -212,6 +214,18 @@ config TC1100_WMI
 	  This is a driver for the WMI extensions (wireless and bluetooth power
 	  control) of the HP Compaq TC1100 tablet.
 
+config HP_WMI
+       tristate "HP WMI extras"
+       depends on ACPI_WMI
+       depends on INPUT
+       depends on RFKILL
+       help
+         Say Y here if you want to support WMI-based hotkeys on HP laptops and
+	 to read data from WMI such as docking or ambient light sensor state.
+
+         To compile this driver as a module, choose M here: the module will
+         be called hp-wmi.
+
 config MSI_LAPTOP
         tristate "MSI Laptop Extras"
         depends on X86
@@ -279,6 +293,8 @@ config THINKPAD_ACPI
 	select INPUT
 	select NEW_LEDS
 	select LEDS_CLASS
+	select NET
+	select RFKILL
 	---help---
 	  This is a driver for the IBM and Lenovo ThinkPad laptops. It adds
 	  support for Fn-Fx key combinations, Bluetooth control, video
@@ -422,6 +438,7 @@ config SGI_XP
 
 config HP_ILO
 	tristate "Channel interface driver for HP iLO/iLO2 processor"
+	depends on PCI
 	default n
 	help
 	  The channel interface driver allows applications to communicate
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 688fe76135e..f5e273420c0 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_ACER_WMI)		+= acer-wmi.o
 obj-$(CONFIG_ATMEL_PWM)		+= atmel_pwm.o
 obj-$(CONFIG_ATMEL_SSC)		+= atmel-ssc.o
 obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
+obj-$(CONFIG_HP_WMI)		+= hp-wmi.o
 obj-$(CONFIG_TC1100_WMI)	+= tc1100-wmi.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
 obj-$(CONFIG_TIFM_CORE)       	+= tifm_core.o
diff --git a/drivers/misc/atmel_pwm.c b/drivers/misc/atmel_pwm.c
index 5b5a14dab3d..6aa5294dfec 100644
--- a/drivers/misc/atmel_pwm.c
+++ b/drivers/misc/atmel_pwm.c
@@ -211,8 +211,7 @@ int pwm_clk_alloc(unsigned prescale, unsigned div)
 	if ((mr & 0xffff) == 0) {
 		mr |= val;
 		ret = PWM_CPR_CLKA;
-	}
-	if ((mr & (0xffff << 16)) == 0) {
+	} else if ((mr & (0xffff << 16)) == 0) {
 		mr |= val << 16;
 		ret = PWM_CPR_CLKB;
 	}
diff --git a/drivers/misc/hp-wmi.c b/drivers/misc/hp-wmi.c
new file mode 100644
index 00000000000..1dbcbcb323a
--- /dev/null
+++ b/drivers/misc/hp-wmi.c
@@ -0,0 +1,494 @@
+/*
+ * HP WMI hotkeys
+ *
+ * Copyright (C) 2008 Red Hat <mjg@redhat.com>
+ *
+ * Portions based on wistron_btns.c:
+ * Copyright (C) 2005 Miloslav Trmac <mitr@volny.cz>
+ * Copyright (C) 2005 Bernhard Rosenkraenzer <bero@arklinux.org>
+ * Copyright (C) 2005 Dmitry Torokhov <dtor@mail.ru>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <acpi/acpi_drivers.h>
+#include <linux/platform_device.h>
+#include <linux/acpi.h>
+#include <linux/rfkill.h>
+#include <linux/string.h>
+
+MODULE_AUTHOR("Matthew Garrett <mjg59@srcf.ucam.org>");
+MODULE_DESCRIPTION("HP laptop WMI hotkeys driver");
+MODULE_LICENSE("GPL");
+
+MODULE_ALIAS("wmi:95F24279-4D7B-4334-9387-ACCDC67EF61C");
+MODULE_ALIAS("wmi:5FB7F034-2C63-45e9-BE91-3D44E2C707E4");
+
+#define HPWMI_EVENT_GUID "95F24279-4D7B-4334-9387-ACCDC67EF61C"
+#define HPWMI_BIOS_GUID "5FB7F034-2C63-45e9-BE91-3D44E2C707E4"
+
+#define HPWMI_DISPLAY_QUERY 0x1
+#define HPWMI_HDDTEMP_QUERY 0x2
+#define HPWMI_ALS_QUERY 0x3
+#define HPWMI_DOCK_QUERY 0x4
+#define HPWMI_WIRELESS_QUERY 0x5
+
+static int __init hp_wmi_bios_setup(struct platform_device *device);
+static int __exit hp_wmi_bios_remove(struct platform_device *device);
+
+struct bios_args {
+	u32 signature;
+	u32 command;
+	u32 commandtype;
+	u32 datasize;
+	u32 data;
+};
+
+struct bios_return {
+	u32 sigpass;
+	u32 return_code;
+	u32 value;
+};
+
+struct key_entry {
+	char type;		/* See KE_* below */
+	u8 code;
+	u16 keycode;
+};
+
+enum { KE_KEY, KE_SW, KE_END };
+
+static struct key_entry hp_wmi_keymap[] = {
+	{KE_SW, 0x01, SW_DOCK},
+	{KE_KEY, 0x02, KEY_BRIGHTNESSUP},
+	{KE_KEY, 0x03, KEY_BRIGHTNESSDOWN},
+	{KE_KEY, 0x04, KEY_HELP},
+	{KE_END, 0}
+};
+
+static struct input_dev *hp_wmi_input_dev;
+static struct platform_device *hp_wmi_platform_dev;
+
+static struct rfkill *wifi_rfkill;
+static struct rfkill *bluetooth_rfkill;
+static struct rfkill *wwan_rfkill;
+
+static struct platform_driver hp_wmi_driver = {
+	.driver = {
+		   .name = "hp-wmi",
+		   .owner = THIS_MODULE,
+	},
+	.probe = hp_wmi_bios_setup,
+	.remove = hp_wmi_bios_remove,
+};
+
+static int hp_wmi_perform_query(int query, int write, int value)
+{
+	struct bios_return bios_return;
+	acpi_status status;
+	union acpi_object *obj;
+	struct bios_args args = {
+		.signature = 0x55434553,
+		.command = write ? 0x2 : 0x1,
+		.commandtype = query,
+		.datasize = write ? 0x4 : 0,
+		.data = value,
+	};
+	struct acpi_buffer input = { sizeof(struct bios_args), &args };
+	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
+
+	status = wmi_evaluate_method(HPWMI_BIOS_GUID, 0, 0x3, &input, &output);
+
+	obj = output.pointer;
+
+	if (!obj || obj->type != ACPI_TYPE_BUFFER)
+		return -EINVAL;
+
+	bios_return = *((struct bios_return *)obj->buffer.pointer);
+	if (bios_return.return_code > 0)
+		return bios_return.return_code * -1;
+	else
+		return bios_return.value;
+}
+
+static int hp_wmi_display_state(void)
+{
+	return hp_wmi_perform_query(HPWMI_DISPLAY_QUERY, 0, 0);
+}
+
+static int hp_wmi_hddtemp_state(void)
+{
+	return hp_wmi_perform_query(HPWMI_HDDTEMP_QUERY, 0, 0);
+}
+
+static int hp_wmi_als_state(void)
+{
+	return hp_wmi_perform_query(HPWMI_ALS_QUERY, 0, 0);
+}
+
+static int hp_wmi_dock_state(void)
+{
+	return hp_wmi_perform_query(HPWMI_DOCK_QUERY, 0, 0);
+}
+
+static int hp_wmi_wifi_set(void *data, enum rfkill_state state)
+{
+	if (state)
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x101);
+	else
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x100);
+}
+
+static int hp_wmi_bluetooth_set(void *data, enum rfkill_state state)
+{
+	if (state)
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x202);
+	else
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x200);
+}
+
+static int hp_wmi_wwan_set(void *data, enum rfkill_state state)
+{
+	if (state)
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x404);
+	else
+		return hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1, 0x400);
+}
+
+static int hp_wmi_wifi_state(void)
+{
+	int wireless = hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0, 0);
+
+	if (wireless & 0x100)
+		return 1;
+	else
+		return 0;
+}
+
+static int hp_wmi_bluetooth_state(void)
+{
+	int wireless = hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0, 0);
+
+	if (wireless & 0x10000)
+		return 1;
+	else
+		return 0;
+}
+
+static int hp_wmi_wwan_state(void)
+{
+	int wireless = hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0, 0);
+
+	if (wireless & 0x1000000)
+		return 1;
+	else
+		return 0;
+}
+
+static ssize_t show_display(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	int value = hp_wmi_display_state();
+	if (value < 0)
+		return -EINVAL;
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t show_hddtemp(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	int value = hp_wmi_hddtemp_state();
+	if (value < 0)
+		return -EINVAL;
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t show_als(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	int value = hp_wmi_als_state();
+	if (value < 0)
+		return -EINVAL;
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t show_dock(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	int value = hp_wmi_dock_state();
+	if (value < 0)
+		return -EINVAL;
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t set_als(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	u32 tmp = simple_strtoul(buf, NULL, 10);
+	hp_wmi_perform_query(HPWMI_ALS_QUERY, 1, tmp);
+	return count;
+}
+
+static DEVICE_ATTR(display, S_IRUGO, show_display, NULL);
+static DEVICE_ATTR(hddtemp, S_IRUGO, show_hddtemp, NULL);
+static DEVICE_ATTR(als, S_IRUGO | S_IWUSR, show_als, set_als);
+static DEVICE_ATTR(dock, S_IRUGO, show_dock, NULL);
+
+static struct key_entry *hp_wmi_get_entry_by_scancode(int code)
+{
+	struct key_entry *key;
+
+	for (key = hp_wmi_keymap; key->type != KE_END; key++)
+		if (code == key->code)
+			return key;
+
+	return NULL;
+}
+
+static struct key_entry *hp_wmi_get_entry_by_keycode(int keycode)
+{
+	struct key_entry *key;
+
+	for (key = hp_wmi_keymap; key->type != KE_END; key++)
+		if (key->type == KE_KEY && keycode == key->keycode)
+			return key;
+
+	return NULL;
+}
+
+static int hp_wmi_getkeycode(struct input_dev *dev, int scancode, int *keycode)
+{
+	struct key_entry *key = hp_wmi_get_entry_by_scancode(scancode);
+
+	if (key && key->type == KE_KEY) {
+		*keycode = key->keycode;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int hp_wmi_setkeycode(struct input_dev *dev, int scancode, int keycode)
+{
+	struct key_entry *key;
+	int old_keycode;
+
+	if (keycode < 0 || keycode > KEY_MAX)
+		return -EINVAL;
+
+	key = hp_wmi_get_entry_by_scancode(scancode);
+	if (key && key->type == KE_KEY) {
+		old_keycode = key->keycode;
+		key->keycode = keycode;
+		set_bit(keycode, dev->keybit);
+		if (!hp_wmi_get_entry_by_keycode(old_keycode))
+			clear_bit(old_keycode, dev->keybit);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+void hp_wmi_notify(u32 value, void *context)
+{
+	struct acpi_buffer response = { ACPI_ALLOCATE_BUFFER, NULL };
+	static struct key_entry *key;
+	union acpi_object *obj;
+
+	wmi_get_event_data(value, &response);
+
+	obj = (union acpi_object *)response.pointer;
+
+	if (obj && obj->type == ACPI_TYPE_BUFFER && obj->buffer.length == 8) {
+		int eventcode = *((u8 *) obj->buffer.pointer);
+		key = hp_wmi_get_entry_by_scancode(eventcode);
+		if (key) {
+			switch (key->type) {
+			case KE_KEY:
+				input_report_key(hp_wmi_input_dev,
+						 key->keycode, 1);
+				input_sync(hp_wmi_input_dev);
+				input_report_key(hp_wmi_input_dev,
+						 key->keycode, 0);
+				input_sync(hp_wmi_input_dev);
+				break;
+			case KE_SW:
+				input_report_switch(hp_wmi_input_dev,
+						    key->keycode,
+						    hp_wmi_dock_state());
+				input_sync(hp_wmi_input_dev);
+				break;
+			}
+		} else if (eventcode == 0x5) {
+			if (wifi_rfkill)
+				wifi_rfkill->state = hp_wmi_wifi_state();
+			if (bluetooth_rfkill)
+				bluetooth_rfkill->state =
+				    hp_wmi_bluetooth_state();
+			if (wwan_rfkill)
+				wwan_rfkill->state = hp_wmi_wwan_state();
+		} else
+			printk(KERN_INFO "HP WMI: Unknown key pressed - %x\n",
+			       eventcode);
+	} else
+		printk(KERN_INFO "HP WMI: Unknown response received\n");
+}
+
+static int __init hp_wmi_input_setup(void)
+{
+	struct key_entry *key;
+	int err;
+
+	hp_wmi_input_dev = input_allocate_device();
+
+	hp_wmi_input_dev->name = "HP WMI hotkeys";
+	hp_wmi_input_dev->phys = "wmi/input0";
+	hp_wmi_input_dev->id.bustype = BUS_HOST;
+	hp_wmi_input_dev->getkeycode = hp_wmi_getkeycode;
+	hp_wmi_input_dev->setkeycode = hp_wmi_setkeycode;
+
+	for (key = hp_wmi_keymap; key->type != KE_END; key++) {
+		switch (key->type) {
+		case KE_KEY:
+			set_bit(EV_KEY, hp_wmi_input_dev->evbit);
+			set_bit(key->keycode, hp_wmi_input_dev->keybit);
+			break;
+		case KE_SW:
+			set_bit(EV_SW, hp_wmi_input_dev->evbit);
+			set_bit(key->keycode, hp_wmi_input_dev->swbit);
+			break;
+		}
+	}
+
+	err = input_register_device(hp_wmi_input_dev);
+
+	if (err) {
+		input_free_device(hp_wmi_input_dev);
+		return err;
+	}
+
+	return 0;
+}
+
+static void cleanup_sysfs(struct platform_device *device)
+{
+	device_remove_file(&device->dev, &dev_attr_display);
+	device_remove_file(&device->dev, &dev_attr_hddtemp);
+	device_remove_file(&device->dev, &dev_attr_als);
+	device_remove_file(&device->dev, &dev_attr_dock);
+}
+
+static int __init hp_wmi_bios_setup(struct platform_device *device)
+{
+	int err;
+
+	err = device_create_file(&device->dev, &dev_attr_display);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_hddtemp);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_als);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_dock);
+	if (err)
+		goto add_sysfs_error;
+
+	wifi_rfkill = rfkill_allocate(&device->dev, RFKILL_TYPE_WLAN);
+	wifi_rfkill->name = "hp-wifi";
+	wifi_rfkill->state = hp_wmi_wifi_state();
+	wifi_rfkill->toggle_radio = hp_wmi_wifi_set;
+	wifi_rfkill->user_claim_unsupported = 1;
+
+	bluetooth_rfkill = rfkill_allocate(&device->dev,
+					   RFKILL_TYPE_BLUETOOTH);
+	bluetooth_rfkill->name = "hp-bluetooth";
+	bluetooth_rfkill->state = hp_wmi_bluetooth_state();
+	bluetooth_rfkill->toggle_radio = hp_wmi_bluetooth_set;
+	bluetooth_rfkill->user_claim_unsupported = 1;
+
+	wwan_rfkill = rfkill_allocate(&device->dev, RFKILL_TYPE_WIMAX);
+	wwan_rfkill->name = "hp-wwan";
+	wwan_rfkill->state = hp_wmi_wwan_state();
+	wwan_rfkill->toggle_radio = hp_wmi_wwan_set;
+	wwan_rfkill->user_claim_unsupported = 1;
+
+	rfkill_register(wifi_rfkill);
+	rfkill_register(bluetooth_rfkill);
+	rfkill_register(wwan_rfkill);
+
+	return 0;
+add_sysfs_error:
+	cleanup_sysfs(device);
+	return err;
+}
+
+static int __exit hp_wmi_bios_remove(struct platform_device *device)
+{
+	cleanup_sysfs(device);
+
+	rfkill_unregister(wifi_rfkill);
+	rfkill_unregister(bluetooth_rfkill);
+	rfkill_unregister(wwan_rfkill);
+
+	return 0;
+}
+
+static int __init hp_wmi_init(void)
+{
+	int err;
+
+	if (wmi_has_guid(HPWMI_EVENT_GUID)) {
+		err = wmi_install_notify_handler(HPWMI_EVENT_GUID,
+						 hp_wmi_notify, NULL);
+		if (!err)
+			hp_wmi_input_setup();
+	}
+
+	if (wmi_has_guid(HPWMI_BIOS_GUID)) {
+		err = platform_driver_register(&hp_wmi_driver);
+		if (err)
+			return 0;
+		hp_wmi_platform_dev = platform_device_alloc("hp-wmi", -1);
+		if (!hp_wmi_platform_dev) {
+			platform_driver_unregister(&hp_wmi_driver);
+			return 0;
+		}
+		platform_device_add(hp_wmi_platform_dev);
+	}
+
+	return 0;
+}
+
+static void __exit hp_wmi_exit(void)
+{
+	if (wmi_has_guid(HPWMI_EVENT_GUID)) {
+		wmi_remove_notify_handler(HPWMI_EVENT_GUID);
+		input_unregister_device(hp_wmi_input_dev);
+	}
+	if (hp_wmi_platform_dev) {
+		platform_device_del(hp_wmi_platform_dev);
+		platform_driver_unregister(&hp_wmi_driver);
+	}
+}
+
+module_init(hp_wmi_init);
+module_exit(hp_wmi_exit);
diff --git a/drivers/misc/phantom.c b/drivers/misc/phantom.c
index 4ce3bdc2f95..daf585689ce 100644
--- a/drivers/misc/phantom.c
+++ b/drivers/misc/phantom.c
@@ -563,6 +563,6 @@ module_init(phantom_init);
 module_exit(phantom_exit);
 
 MODULE_AUTHOR("Jiri Slaby <jirislaby@gmail.com>");
-MODULE_DESCRIPTION("Sensable Phantom driver");
+MODULE_DESCRIPTION("Sensable Phantom driver (PCI devices)");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(PHANTOM_VERSION);
diff --git a/drivers/misc/thinkpad_acpi.c b/drivers/misc/thinkpad_acpi.c
index b5969298f3d..d3eb7903c34 100644
--- a/drivers/misc/thinkpad_acpi.c
+++ b/drivers/misc/thinkpad_acpi.c
@@ -21,7 +21,7 @@
  *  02110-1301, USA.
  */
 
-#define TPACPI_VERSION "0.20"
+#define TPACPI_VERSION "0.21"
 #define TPACPI_SYSFS_VERSION 0x020200
 
 /*
@@ -68,6 +68,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/input.h>
 #include <linux/leds.h>
+#include <linux/rfkill.h>
 #include <asm/uaccess.h>
 
 #include <linux/dmi.h>
@@ -144,6 +145,12 @@ enum {
 
 #define TPACPI_MAX_ACPI_ARGS 3
 
+/* rfkill switches */
+enum {
+	TPACPI_RFK_BLUETOOTH_SW_ID = 0,
+	TPACPI_RFK_WWAN_SW_ID,
+};
+
 /* Debugging */
 #define TPACPI_LOG TPACPI_FILE ": "
 #define TPACPI_ERR	   KERN_ERR    TPACPI_LOG
@@ -905,6 +912,43 @@ static int __init tpacpi_check_std_acpi_brightness_support(void)
 	return 0;
 }
 
+static int __init tpacpi_new_rfkill(const unsigned int id,
+			struct rfkill **rfk,
+			const enum rfkill_type rfktype,
+			const char *name,
+			int (*toggle_radio)(void *, enum rfkill_state),
+			int (*get_state)(void *, enum rfkill_state *))
+{
+	int res;
+	enum rfkill_state initial_state;
+
+	*rfk = rfkill_allocate(&tpacpi_pdev->dev, rfktype);
+	if (!*rfk) {
+		printk(TPACPI_ERR
+			"failed to allocate memory for rfkill class\n");
+		return -ENOMEM;
+	}
+
+	(*rfk)->name = name;
+	(*rfk)->get_state = get_state;
+	(*rfk)->toggle_radio = toggle_radio;
+
+	if (!get_state(NULL, &initial_state))
+		(*rfk)->state = initial_state;
+
+	res = rfkill_register(*rfk);
+	if (res < 0) {
+		printk(TPACPI_ERR
+			"failed to register %s rfkill switch: %d\n",
+			name, res);
+		rfkill_free(*rfk);
+		*rfk = NULL;
+		return res;
+	}
+
+	return 0;
+}
+
 /*************************************************************************
  * thinkpad-acpi driver attributes
  */
@@ -1285,21 +1329,6 @@ static int hotkey_status_set(int status)
 	return 0;
 }
 
-static void tpacpi_input_send_radiosw(void)
-{
-	int wlsw;
-
-	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&wlsw)) {
-		mutex_lock(&tpacpi_inputdev_send_mutex);
-
-		input_report_switch(tpacpi_inputdev,
-				    SW_RFKILL_ALL, !!wlsw);
-		input_sync(tpacpi_inputdev);
-
-		mutex_unlock(&tpacpi_inputdev_send_mutex);
-	}
-}
-
 static void tpacpi_input_send_tabletsw(void)
 {
 	int state;
@@ -1921,6 +1950,30 @@ static struct attribute *hotkey_mask_attributes[] __initdata = {
 	&dev_attr_hotkey_wakeup_hotunplug_complete.attr,
 };
 
+static void bluetooth_update_rfk(void);
+static void wan_update_rfk(void);
+static void tpacpi_send_radiosw_update(void)
+{
+	int wlsw;
+
+	/* Sync these BEFORE sending any rfkill events */
+	if (tp_features.bluetooth)
+		bluetooth_update_rfk();
+	if (tp_features.wan)
+		wan_update_rfk();
+
+	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&wlsw)) {
+		mutex_lock(&tpacpi_inputdev_send_mutex);
+
+		input_report_switch(tpacpi_inputdev,
+				    SW_RFKILL_ALL, !!wlsw);
+		input_sync(tpacpi_inputdev);
+
+		mutex_unlock(&tpacpi_inputdev_send_mutex);
+	}
+	hotkey_radio_sw_notify_change();
+}
+
 static void hotkey_exit(void)
 {
 #ifdef CONFIG_THINKPAD_ACPI_HOTKEY_POLL
@@ -2167,9 +2220,10 @@ static int __init hotkey_init(struct ibm_init_struct *iibm)
 		printk(TPACPI_INFO
 			"radio switch found; radios are %s\n",
 			enabled(status, 0));
+	}
+	if (tp_features.hotkey_wlsw)
 		res = add_to_attr_set(hotkey_dev_attributes,
 				&dev_attr_hotkey_radio_sw.attr);
-	}
 
 	/* For X41t, X60t, X61t Tablets... */
 	if (!res && acpi_evalf(hkey_handle, &status, "MHKG", "qd")) {
@@ -2287,7 +2341,7 @@ static int __init hotkey_init(struct ibm_init_struct *iibm)
 	tpacpi_inputdev->close = &hotkey_inputdev_close;
 
 	hotkey_poll_setup_safe(1);
-	tpacpi_input_send_radiosw();
+	tpacpi_send_radiosw_update();
 	tpacpi_input_send_tabletsw();
 
 	return 0;
@@ -2419,8 +2473,7 @@ static void hotkey_notify(struct ibm_struct *ibm, u32 event)
 		case 7:
 			/* 0x7000-0x7FFF: misc */
 			if (tp_features.hotkey_wlsw && hkey == 0x7000) {
-				tpacpi_input_send_radiosw();
-				hotkey_radio_sw_notify_change();
+				tpacpi_send_radiosw_update();
 				send_acpi_ev = 0;
 				break;
 			}
@@ -2463,8 +2516,7 @@ static void hotkey_resume(void)
 		printk(TPACPI_ERR
 		       "error while trying to read hot key mask "
 		       "from firmware\n");
-	tpacpi_input_send_radiosw();
-	hotkey_radio_sw_notify_change();
+	tpacpi_send_radiosw_update();
 	hotkey_tablet_mode_notify_change();
 	hotkey_wakeup_reason_notify_change();
 	hotkey_wakeup_hotunplug_complete_notify_change();
@@ -2581,8 +2633,66 @@ enum {
 	TP_ACPI_BLUETOOTH_UNK		= 0x04,	/* unknown function */
 };
 
-static int bluetooth_get_radiosw(void);
-static int bluetooth_set_radiosw(int radio_on);
+static struct rfkill *tpacpi_bluetooth_rfkill;
+
+static int bluetooth_get_radiosw(void)
+{
+	int status;
+
+	if (!tp_features.bluetooth)
+		return -ENODEV;
+
+	/* WLSW overrides bluetooth in firmware/hardware, reflect that */
+	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&status) && !status)
+		return RFKILL_STATE_HARD_BLOCKED;
+
+	if (!acpi_evalf(hkey_handle, &status, "GBDC", "d"))
+		return -EIO;
+
+	return ((status & TP_ACPI_BLUETOOTH_RADIOSSW) != 0) ?
+		RFKILL_STATE_UNBLOCKED : RFKILL_STATE_SOFT_BLOCKED;
+}
+
+static void bluetooth_update_rfk(void)
+{
+	int status;
+
+	if (!tpacpi_bluetooth_rfkill)
+		return;
+
+	status = bluetooth_get_radiosw();
+	if (status < 0)
+		return;
+	rfkill_force_state(tpacpi_bluetooth_rfkill, status);
+}
+
+static int bluetooth_set_radiosw(int radio_on, int update_rfk)
+{
+	int status;
+
+	if (!tp_features.bluetooth)
+		return -ENODEV;
+
+	/* WLSW overrides bluetooth in firmware/hardware, but there is no
+	 * reason to risk weird behaviour. */
+	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&status) && !status
+	    && radio_on)
+		return -EPERM;
+
+	if (!acpi_evalf(hkey_handle, &status, "GBDC", "d"))
+		return -EIO;
+	if (radio_on)
+		status |= TP_ACPI_BLUETOOTH_RADIOSSW;
+	else
+		status &= ~TP_ACPI_BLUETOOTH_RADIOSSW;
+	if (!acpi_evalf(hkey_handle, NULL, "SBDC", "vd", status))
+		return -EIO;
+
+	if (update_rfk)
+		bluetooth_update_rfk();
+
+	return 0;
+}
 
 /* sysfs bluetooth enable ---------------------------------------------- */
 static ssize_t bluetooth_enable_show(struct device *dev,
@@ -2595,7 +2705,8 @@ static ssize_t bluetooth_enable_show(struct device *dev,
 	if (status < 0)
 		return status;
 
-	return snprintf(buf, PAGE_SIZE, "%d\n", status ? 1 : 0);
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			(status == RFKILL_STATE_UNBLOCKED) ? 1 : 0);
 }
 
 static ssize_t bluetooth_enable_store(struct device *dev,
@@ -2608,7 +2719,7 @@ static ssize_t bluetooth_enable_store(struct device *dev,
 	if (parse_strtoul(buf, 1, &t))
 		return -EINVAL;
 
-	res = bluetooth_set_radiosw(t);
+	res = bluetooth_set_radiosw(t, 1);
 
 	return (res) ? res : count;
 }
@@ -2628,6 +2739,31 @@ static const struct attribute_group bluetooth_attr_group = {
 	.attrs = bluetooth_attributes,
 };
 
+static int tpacpi_bluetooth_rfk_get(void *data, enum rfkill_state *state)
+{
+	int bts = bluetooth_get_radiosw();
+
+	if (bts < 0)
+		return bts;
+
+	*state = bts;
+	return 0;
+}
+
+static int tpacpi_bluetooth_rfk_set(void *data, enum rfkill_state state)
+{
+	return bluetooth_set_radiosw((state == RFKILL_STATE_UNBLOCKED), 0);
+}
+
+static void bluetooth_exit(void)
+{
+	if (tpacpi_bluetooth_rfkill)
+		rfkill_unregister(tpacpi_bluetooth_rfkill);
+
+	sysfs_remove_group(&tpacpi_pdev->dev.kobj,
+			&bluetooth_attr_group);
+}
+
 static int __init bluetooth_init(struct ibm_init_struct *iibm)
 {
 	int res;
@@ -2646,57 +2782,32 @@ static int __init bluetooth_init(struct ibm_init_struct *iibm)
 		str_supported(tp_features.bluetooth),
 		status);
 
-	if (tp_features.bluetooth) {
-		if (!(status & TP_ACPI_BLUETOOTH_HWPRESENT)) {
-			/* no bluetooth hardware present in system */
-			tp_features.bluetooth = 0;
-			dbg_printk(TPACPI_DBG_INIT,
-				   "bluetooth hardware not installed\n");
-		} else {
-			res = sysfs_create_group(&tpacpi_pdev->dev.kobj,
-					&bluetooth_attr_group);
-			if (res)
-				return res;
-		}
+	if (tp_features.bluetooth &&
+	    !(status & TP_ACPI_BLUETOOTH_HWPRESENT)) {
+		/* no bluetooth hardware present in system */
+		tp_features.bluetooth = 0;
+		dbg_printk(TPACPI_DBG_INIT,
+			   "bluetooth hardware not installed\n");
 	}
 
-	return (tp_features.bluetooth)? 0 : 1;
-}
-
-static void bluetooth_exit(void)
-{
-	sysfs_remove_group(&tpacpi_pdev->dev.kobj,
-			&bluetooth_attr_group);
-}
-
-static int bluetooth_get_radiosw(void)
-{
-	int status;
-
 	if (!tp_features.bluetooth)
-		return -ENODEV;
-
-	if (!acpi_evalf(hkey_handle, &status, "GBDC", "d"))
-		return -EIO;
-
-	return ((status & TP_ACPI_BLUETOOTH_RADIOSSW) != 0);
-}
-
-static int bluetooth_set_radiosw(int radio_on)
-{
-	int status;
+		return 1;
 
-	if (!tp_features.bluetooth)
-		return -ENODEV;
+	res = sysfs_create_group(&tpacpi_pdev->dev.kobj,
+				&bluetooth_attr_group);
+	if (res)
+		return res;
 
-	if (!acpi_evalf(hkey_handle, &status, "GBDC", "d"))
-		return -EIO;
-	if (radio_on)
-		status |= TP_ACPI_BLUETOOTH_RADIOSSW;
-	else
-		status &= ~TP_ACPI_BLUETOOTH_RADIOSSW;
-	if (!acpi_evalf(hkey_handle, NULL, "SBDC", "vd", status))
-		return -EIO;
+	res = tpacpi_new_rfkill(TPACPI_RFK_BLUETOOTH_SW_ID,
+				&tpacpi_bluetooth_rfkill,
+				RFKILL_TYPE_BLUETOOTH,
+				"tpacpi_bluetooth_sw",
+				tpacpi_bluetooth_rfk_set,
+				tpacpi_bluetooth_rfk_get);
+	if (res) {
+		bluetooth_exit();
+		return res;
+	}
 
 	return 0;
 }
@@ -2711,7 +2822,8 @@ static int bluetooth_read(char *p)
 		len += sprintf(p + len, "status:\t\tnot supported\n");
 	else {
 		len += sprintf(p + len, "status:\t\t%s\n",
-				(status)? "enabled" : "disabled");
+				(status == RFKILL_STATE_UNBLOCKED) ?
+					"enabled" : "disabled");
 		len += sprintf(p + len, "commands:\tenable, disable\n");
 	}
 
@@ -2727,9 +2839,9 @@ static int bluetooth_write(char *buf)
 
 	while ((cmd = next_cmd(&buf))) {
 		if (strlencmp(cmd, "enable") == 0) {
-			bluetooth_set_radiosw(1);
+			bluetooth_set_radiosw(1, 1);
 		} else if (strlencmp(cmd, "disable") == 0) {
-			bluetooth_set_radiosw(0);
+			bluetooth_set_radiosw(0, 1);
 		} else
 			return -EINVAL;
 	}
@@ -2755,8 +2867,66 @@ enum {
 	TP_ACPI_WANCARD_UNK		= 0x04,	/* unknown function */
 };
 
-static int wan_get_radiosw(void);
-static int wan_set_radiosw(int radio_on);
+static struct rfkill *tpacpi_wan_rfkill;
+
+static int wan_get_radiosw(void)
+{
+	int status;
+
+	if (!tp_features.wan)
+		return -ENODEV;
+
+	/* WLSW overrides WWAN in firmware/hardware, reflect that */
+	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&status) && !status)
+		return RFKILL_STATE_HARD_BLOCKED;
+
+	if (!acpi_evalf(hkey_handle, &status, "GWAN", "d"))
+		return -EIO;
+
+	return ((status & TP_ACPI_WANCARD_RADIOSSW) != 0) ?
+		RFKILL_STATE_UNBLOCKED : RFKILL_STATE_SOFT_BLOCKED;
+}
+
+static void wan_update_rfk(void)
+{
+	int status;
+
+	if (!tpacpi_wan_rfkill)
+		return;
+
+	status = wan_get_radiosw();
+	if (status < 0)
+		return;
+	rfkill_force_state(tpacpi_wan_rfkill, status);
+}
+
+static int wan_set_radiosw(int radio_on, int update_rfk)
+{
+	int status;
+
+	if (!tp_features.wan)
+		return -ENODEV;
+
+	/* WLSW overrides bluetooth in firmware/hardware, but there is no
+	 * reason to risk weird behaviour. */
+	if (tp_features.hotkey_wlsw && !hotkey_get_wlsw(&status) && !status
+	    && radio_on)
+		return -EPERM;
+
+	if (!acpi_evalf(hkey_handle, &status, "GWAN", "d"))
+		return -EIO;
+	if (radio_on)
+		status |= TP_ACPI_WANCARD_RADIOSSW;
+	else
+		status &= ~TP_ACPI_WANCARD_RADIOSSW;
+	if (!acpi_evalf(hkey_handle, NULL, "SWAN", "vd", status))
+		return -EIO;
+
+	if (update_rfk)
+		wan_update_rfk();
+
+	return 0;
+}
 
 /* sysfs wan enable ---------------------------------------------------- */
 static ssize_t wan_enable_show(struct device *dev,
@@ -2769,7 +2939,8 @@ static ssize_t wan_enable_show(struct device *dev,
 	if (status < 0)
 		return status;
 
-	return snprintf(buf, PAGE_SIZE, "%d\n", status ? 1 : 0);
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			(status == RFKILL_STATE_UNBLOCKED) ? 1 : 0);
 }
 
 static ssize_t wan_enable_store(struct device *dev,
@@ -2782,7 +2953,7 @@ static ssize_t wan_enable_store(struct device *dev,
 	if (parse_strtoul(buf, 1, &t))
 		return -EINVAL;
 
-	res = wan_set_radiosw(t);
+	res = wan_set_radiosw(t, 1);
 
 	return (res) ? res : count;
 }
@@ -2802,6 +2973,31 @@ static const struct attribute_group wan_attr_group = {
 	.attrs = wan_attributes,
 };
 
+static int tpacpi_wan_rfk_get(void *data, enum rfkill_state *state)
+{
+	int wans = wan_get_radiosw();
+
+	if (wans < 0)
+		return wans;
+
+	*state = wans;
+	return 0;
+}
+
+static int tpacpi_wan_rfk_set(void *data, enum rfkill_state state)
+{
+	return wan_set_radiosw((state == RFKILL_STATE_UNBLOCKED), 0);
+}
+
+static void wan_exit(void)
+{
+	if (tpacpi_wan_rfkill)
+		rfkill_unregister(tpacpi_wan_rfkill);
+
+	sysfs_remove_group(&tpacpi_pdev->dev.kobj,
+		&wan_attr_group);
+}
+
 static int __init wan_init(struct ibm_init_struct *iibm)
 {
 	int res;
@@ -2818,57 +3014,32 @@ static int __init wan_init(struct ibm_init_struct *iibm)
 		str_supported(tp_features.wan),
 		status);
 
-	if (tp_features.wan) {
-		if (!(status & TP_ACPI_WANCARD_HWPRESENT)) {
-			/* no wan hardware present in system */
-			tp_features.wan = 0;
-			dbg_printk(TPACPI_DBG_INIT,
-				   "wan hardware not installed\n");
-		} else {
-			res = sysfs_create_group(&tpacpi_pdev->dev.kobj,
-					&wan_attr_group);
-			if (res)
-				return res;
-		}
+	if (tp_features.wan &&
+	    !(status & TP_ACPI_WANCARD_HWPRESENT)) {
+		/* no wan hardware present in system */
+		tp_features.wan = 0;
+		dbg_printk(TPACPI_DBG_INIT,
+			   "wan hardware not installed\n");
 	}
 
-	return (tp_features.wan)? 0 : 1;
-}
-
-static void wan_exit(void)
-{
-	sysfs_remove_group(&tpacpi_pdev->dev.kobj,
-		&wan_attr_group);
-}
-
-static int wan_get_radiosw(void)
-{
-	int status;
-
 	if (!tp_features.wan)
-		return -ENODEV;
-
-	if (!acpi_evalf(hkey_handle, &status, "GWAN", "d"))
-		return -EIO;
-
-	return ((status & TP_ACPI_WANCARD_RADIOSSW) != 0);
-}
-
-static int wan_set_radiosw(int radio_on)
-{
-	int status;
+		return 1;
 
-	if (!tp_features.wan)
-		return -ENODEV;
+	res = sysfs_create_group(&tpacpi_pdev->dev.kobj,
+				&wan_attr_group);
+	if (res)
+		return res;
 
-	if (!acpi_evalf(hkey_handle, &status, "GWAN", "d"))
-		return -EIO;
-	if (radio_on)
-		status |= TP_ACPI_WANCARD_RADIOSSW;
-	else
-		status &= ~TP_ACPI_WANCARD_RADIOSSW;
-	if (!acpi_evalf(hkey_handle, NULL, "SWAN", "vd", status))
-		return -EIO;
+	res = tpacpi_new_rfkill(TPACPI_RFK_WWAN_SW_ID,
+				&tpacpi_wan_rfkill,
+				RFKILL_TYPE_WWAN,
+				"tpacpi_wwan_sw",
+				tpacpi_wan_rfk_set,
+				tpacpi_wan_rfk_get);
+	if (res) {
+		wan_exit();
+		return res;
+	}
 
 	return 0;
 }
@@ -2883,7 +3054,8 @@ static int wan_read(char *p)
 		len += sprintf(p + len, "status:\t\tnot supported\n");
 	else {
 		len += sprintf(p + len, "status:\t\t%s\n",
-				(status)? "enabled" : "disabled");
+				(status == RFKILL_STATE_UNBLOCKED) ?
+					"enabled" : "disabled");
 		len += sprintf(p + len, "commands:\tenable, disable\n");
 	}
 
@@ -2899,9 +3071,9 @@ static int wan_write(char *buf)
 
 	while ((cmd = next_cmd(&buf))) {
 		if (strlencmp(cmd, "enable") == 0) {
-			wan_set_radiosw(1);
+			wan_set_radiosw(1, 1);
 		} else if (strlencmp(cmd, "disable") == 0) {
-			wan_set_radiosw(0);
+			wan_set_radiosw(0, 1);
 		} else
 			return -EINVAL;
 	}
@@ -6168,13 +6340,18 @@ err_out:
 
 /* Probing */
 
-static void __init get_thinkpad_model_data(struct thinkpad_id_data *tp)
+/* returns 0 - probe ok, or < 0 - probe error.
+ * Probe ok doesn't mean thinkpad found.
+ * On error, kfree() cleanup on tp->* is not performed, caller must do it */
+static int __must_check __init get_thinkpad_model_data(
+						struct thinkpad_id_data *tp)
 {
 	const struct dmi_device *dev = NULL;
 	char ec_fw_string[18];
+	char const *s;
 
 	if (!tp)
-		return;
+		return -EINVAL;
 
 	memset(tp, 0, sizeof(*tp));
 
@@ -6183,12 +6360,14 @@ static void __init get_thinkpad_model_data(struct thinkpad_id_data *tp)
 	else if (dmi_name_in_vendors("LENOVO"))
 		tp->vendor = PCI_VENDOR_ID_LENOVO;
 	else
-		return;
+		return 0;
 
-	tp->bios_version_str = kstrdup(dmi_get_system_info(DMI_BIOS_VERSION),
-					GFP_KERNEL);
+	s = dmi_get_system_info(DMI_BIOS_VERSION);
+	tp->bios_version_str = kstrdup(s, GFP_KERNEL);
+	if (s && !tp->bios_version_str)
+		return -ENOMEM;
 	if (!tp->bios_version_str)
-		return;
+		return 0;
 	tp->bios_model = tp->bios_version_str[0]
 			 | (tp->bios_version_str[1] << 8);
 
@@ -6207,21 +6386,27 @@ static void __init get_thinkpad_model_data(struct thinkpad_id_data *tp)
 			ec_fw_string[strcspn(ec_fw_string, " ]")] = 0;
 
 			tp->ec_version_str = kstrdup(ec_fw_string, GFP_KERNEL);
+			if (!tp->ec_version_str)
+				return -ENOMEM;
 			tp->ec_model = ec_fw_string[0]
 					| (ec_fw_string[1] << 8);
 			break;
 		}
 	}
 
-	tp->model_str = kstrdup(dmi_get_system_info(DMI_PRODUCT_VERSION),
-					GFP_KERNEL);
-	if (tp->model_str && strnicmp(tp->model_str, "ThinkPad", 8) != 0) {
-		kfree(tp->model_str);
-		tp->model_str = NULL;
+	s = dmi_get_system_info(DMI_PRODUCT_VERSION);
+	if (s && !strnicmp(s, "ThinkPad", 8)) {
+		tp->model_str = kstrdup(s, GFP_KERNEL);
+		if (!tp->model_str)
+			return -ENOMEM;
 	}
 
-	tp->nummodel_str = kstrdup(dmi_get_system_info(DMI_PRODUCT_NAME),
-					GFP_KERNEL);
+	s = dmi_get_system_info(DMI_PRODUCT_NAME);
+	tp->nummodel_str = kstrdup(s, GFP_KERNEL);
+	if (s && !tp->nummodel_str)
+		return -ENOMEM;
+
+	return 0;
 }
 
 static int __init probe_for_thinkpad(void)
@@ -6484,7 +6669,13 @@ static int __init thinkpad_acpi_module_init(void)
 
 	/* Driver-level probe */
 
-	get_thinkpad_model_data(&thinkpad_id);
+	ret = get_thinkpad_model_data(&thinkpad_id);
+	if (ret) {
+		printk(TPACPI_ERR
+			"unable to get DMI data: %d\n", ret);
+		thinkpad_acpi_module_exit();
+		return ret;
+	}
 	ret = probe_for_thinkpad();
 	if (ret) {
 		thinkpad_acpi_module_exit();
diff --git a/drivers/mmc/core/Makefile b/drivers/mmc/core/Makefile
index 19a1a254a0c..889e5f898f6 100644
--- a/drivers/mmc/core/Makefile
+++ b/drivers/mmc/core/Makefile
@@ -12,3 +12,4 @@ mmc_core-y			:= core.o bus.o host.o \
 				   sdio.o sdio_ops.o sdio_bus.o \
 				   sdio_cis.o sdio_io.o sdio_irq.o
 
+mmc_core-$(CONFIG_DEBUG_FS)	+= debugfs.o
diff --git a/drivers/mmc/core/bus.c b/drivers/mmc/core/bus.c
index fd95b18e988..0d9b2d6f9eb 100644
--- a/drivers/mmc/core/bus.c
+++ b/drivers/mmc/core/bus.c
@@ -252,6 +252,10 @@ int mmc_add_card(struct mmc_card *card)
 	if (ret)
 		return ret;
 
+#ifdef CONFIG_DEBUG_FS
+	mmc_add_card_debugfs(card);
+#endif
+
 	mmc_card_set_present(card);
 
 	return 0;
@@ -263,6 +267,10 @@ int mmc_add_card(struct mmc_card *card)
  */
 void mmc_remove_card(struct mmc_card *card)
 {
+#ifdef CONFIG_DEBUG_FS
+	mmc_remove_card_debugfs(card);
+#endif
+
 	if (mmc_card_present(card)) {
 		if (mmc_host_is_spi(card->host)) {
 			printk(KERN_INFO "%s: SPI card removed\n",
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index cdb332b7ded..c819effa103 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -52,5 +52,12 @@ int mmc_attach_sdio(struct mmc_host *host, u32 ocr);
 
 extern int use_spi_crc;
 
+/* Debugfs information for hosts and cards */
+void mmc_add_host_debugfs(struct mmc_host *host);
+void mmc_remove_host_debugfs(struct mmc_host *host);
+
+void mmc_add_card_debugfs(struct mmc_card *card);
+void mmc_remove_card_debugfs(struct mmc_card *card);
+
 #endif
 
diff --git a/drivers/mmc/core/debugfs.c b/drivers/mmc/core/debugfs.c
new file mode 100644
index 00000000000..1237bb4c722
--- /dev/null
+++ b/drivers/mmc/core/debugfs.c
@@ -0,0 +1,225 @@
+/*
+ * Debugfs support for hosts and cards
+ *
+ * Copyright (C) 2008 Atmel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/seq_file.h>
+#include <linux/stat.h>
+
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+
+#include "core.h"
+#include "mmc_ops.h"
+
+/* The debugfs functions are optimized away when CONFIG_DEBUG_FS isn't set. */
+static int mmc_ios_show(struct seq_file *s, void *data)
+{
+	static const char *vdd_str[] = {
+		[8]	= "2.0",
+		[9]	= "2.1",
+		[10]	= "2.2",
+		[11]	= "2.3",
+		[12]	= "2.4",
+		[13]	= "2.5",
+		[14]	= "2.6",
+		[15]	= "2.7",
+		[16]	= "2.8",
+		[17]	= "2.9",
+		[18]	= "3.0",
+		[19]	= "3.1",
+		[20]	= "3.2",
+		[21]	= "3.3",
+		[22]	= "3.4",
+		[23]	= "3.5",
+		[24]	= "3.6",
+	};
+	struct mmc_host	*host = s->private;
+	struct mmc_ios	*ios = &host->ios;
+	const char *str;
+
+	seq_printf(s, "clock:\t\t%u Hz\n", ios->clock);
+	seq_printf(s, "vdd:\t\t%u ", ios->vdd);
+	if ((1 << ios->vdd) & MMC_VDD_165_195)
+		seq_printf(s, "(1.65 - 1.95 V)\n");
+	else if (ios->vdd < (ARRAY_SIZE(vdd_str) - 1)
+			&& vdd_str[ios->vdd] && vdd_str[ios->vdd + 1])
+		seq_printf(s, "(%s ~ %s V)\n", vdd_str[ios->vdd],
+				vdd_str[ios->vdd + 1]);
+	else
+		seq_printf(s, "(invalid)\n");
+
+	switch (ios->bus_mode) {
+	case MMC_BUSMODE_OPENDRAIN:
+		str = "open drain";
+		break;
+	case MMC_BUSMODE_PUSHPULL:
+		str = "push-pull";
+		break;
+	default:
+		str = "invalid";
+		break;
+	}
+	seq_printf(s, "bus mode:\t%u (%s)\n", ios->bus_mode, str);
+
+	switch (ios->chip_select) {
+	case MMC_CS_DONTCARE:
+		str = "don't care";
+		break;
+	case MMC_CS_HIGH:
+		str = "active high";
+		break;
+	case MMC_CS_LOW:
+		str = "active low";
+		break;
+	default:
+		str = "invalid";
+		break;
+	}
+	seq_printf(s, "chip select:\t%u (%s)\n", ios->chip_select, str);
+
+	switch (ios->power_mode) {
+	case MMC_POWER_OFF:
+		str = "off";
+		break;
+	case MMC_POWER_UP:
+		str = "up";
+		break;
+	case MMC_POWER_ON:
+		str = "on";
+		break;
+	default:
+		str = "invalid";
+		break;
+	}
+	seq_printf(s, "power mode:\t%u (%s)\n", ios->power_mode, str);
+	seq_printf(s, "bus width:\t%u (%u bits)\n",
+			ios->bus_width, 1 << ios->bus_width);
+
+	switch (ios->timing) {
+	case MMC_TIMING_LEGACY:
+		str = "legacy";
+		break;
+	case MMC_TIMING_MMC_HS:
+		str = "mmc high-speed";
+		break;
+	case MMC_TIMING_SD_HS:
+		str = "sd high-speed";
+		break;
+	default:
+		str = "invalid";
+		break;
+	}
+	seq_printf(s, "timing spec:\t%u (%s)\n", ios->timing, str);
+
+	return 0;
+}
+
+static int mmc_ios_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mmc_ios_show, inode->i_private);
+}
+
+static const struct file_operations mmc_ios_fops = {
+	.open		= mmc_ios_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void mmc_add_host_debugfs(struct mmc_host *host)
+{
+	struct dentry *root;
+
+	root = debugfs_create_dir(mmc_hostname(host), NULL);
+	if (IS_ERR(root))
+		/* Don't complain -- debugfs just isn't enabled */
+		return;
+	if (!root)
+		/* Complain -- debugfs is enabled, but it failed to
+		 * create the directory. */
+		goto err_root;
+
+	host->debugfs_root = root;
+
+	if (!debugfs_create_file("ios", S_IRUSR, root, host, &mmc_ios_fops))
+		goto err_ios;
+
+	return;
+
+err_ios:
+	debugfs_remove_recursive(root);
+	host->debugfs_root = NULL;
+err_root:
+	dev_err(&host->class_dev, "failed to initialize debugfs\n");
+}
+
+void mmc_remove_host_debugfs(struct mmc_host *host)
+{
+	debugfs_remove_recursive(host->debugfs_root);
+}
+
+static int mmc_dbg_card_status_get(void *data, u64 *val)
+{
+	struct mmc_card	*card = data;
+	u32		status;
+	int		ret;
+
+	mmc_claim_host(card->host);
+
+	ret = mmc_send_status(data, &status);
+	if (!ret)
+		*val = status;
+
+	mmc_release_host(card->host);
+
+	return ret;
+}
+DEFINE_SIMPLE_ATTRIBUTE(mmc_dbg_card_status_fops, mmc_dbg_card_status_get,
+		NULL, "%08llx\n");
+
+void mmc_add_card_debugfs(struct mmc_card *card)
+{
+	struct mmc_host	*host = card->host;
+	struct dentry	*root;
+
+	if (!host->debugfs_root)
+		return;
+
+	root = debugfs_create_dir(mmc_card_id(card), host->debugfs_root);
+	if (IS_ERR(root))
+		/* Don't complain -- debugfs just isn't enabled */
+		return;
+	if (!root)
+		/* Complain -- debugfs is enabled, but it failed to
+		 * create the directory. */
+		goto err;
+
+	card->debugfs_root = root;
+
+	if (!debugfs_create_x32("state", S_IRUSR, root, &card->state))
+		goto err;
+
+	if (mmc_card_mmc(card) || mmc_card_sd(card))
+		if (!debugfs_create_file("status", S_IRUSR, root, card,
+					&mmc_dbg_card_status_fops))
+			goto err;
+
+	return;
+
+err:
+	debugfs_remove_recursive(root);
+	card->debugfs_root = NULL;
+	dev_err(&card->dev, "failed to initialize debugfs\n");
+}
+
+void mmc_remove_card_debugfs(struct mmc_card *card)
+{
+	debugfs_remove_recursive(card->debugfs_root);
+}
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 1d795c5379b..6da80fd4d97 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -127,6 +127,10 @@ int mmc_add_host(struct mmc_host *host)
 	if (err)
 		return err;
 
+#ifdef CONFIG_DEBUG_FS
+	mmc_add_host_debugfs(host);
+#endif
+
 	mmc_start_host(host);
 
 	return 0;
@@ -146,6 +150,10 @@ void mmc_remove_host(struct mmc_host *host)
 {
 	mmc_stop_host(host);
 
+#ifdef CONFIG_DEBUG_FS
+	mmc_remove_host_debugfs(host);
+#endif
+
 	device_del(&host->class_dev);
 
 	led_trigger_unregister_simple(host->led);
diff --git a/drivers/mmc/host/atmel-mci-regs.h b/drivers/mmc/host/atmel-mci-regs.h
index a9a5657706c..26bd80e6503 100644
--- a/drivers/mmc/host/atmel-mci-regs.h
+++ b/drivers/mmc/host/atmel-mci-regs.h
@@ -82,6 +82,8 @@
 # define MCI_OVRE		(  1 <<  30)	/* RX Overrun Error */
 # define MCI_UNRE		(  1 <<  31)	/* TX Underrun Error */
 
+#define MCI_REGS_SIZE		0x100
+
 /* Register access macros */
 #define mci_readl(port,reg)				\
 	__raw_readl((port)->regs + MCI_##reg)
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index cce873c5a14..b68381f7bfd 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -9,6 +9,7 @@
  */
 #include <linux/blkdev.h>
 #include <linux/clk.h>
+#include <linux/debugfs.h>
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
@@ -16,6 +17,8 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/scatterlist.h>
+#include <linux/seq_file.h>
+#include <linux/stat.h>
 
 #include <linux/mmc/host.h>
 
@@ -88,6 +91,188 @@ struct atmel_mci {
 #define atmci_clear_pending(host, event)			\
 	clear_bit(event, &host->pending_events)
 
+/*
+ * The debugfs stuff below is mostly optimized away when
+ * CONFIG_DEBUG_FS is not set.
+ */
+static int atmci_req_show(struct seq_file *s, void *v)
+{
+	struct atmel_mci	*host = s->private;
+	struct mmc_request	*mrq = host->mrq;
+	struct mmc_command	*cmd;
+	struct mmc_command	*stop;
+	struct mmc_data		*data;
+
+	/* Make sure we get a consistent snapshot */
+	spin_lock_irq(&host->mmc->lock);
+
+	if (mrq) {
+		cmd = mrq->cmd;
+		data = mrq->data;
+		stop = mrq->stop;
+
+		if (cmd)
+			seq_printf(s,
+				"CMD%u(0x%x) flg %x rsp %x %x %x %x err %d\n",
+				cmd->opcode, cmd->arg, cmd->flags,
+				cmd->resp[0], cmd->resp[1], cmd->resp[2],
+				cmd->resp[2], cmd->error);
+		if (data)
+			seq_printf(s, "DATA %u / %u * %u flg %x err %d\n",
+				data->bytes_xfered, data->blocks,
+				data->blksz, data->flags, data->error);
+		if (stop)
+			seq_printf(s,
+				"CMD%u(0x%x) flg %x rsp %x %x %x %x err %d\n",
+				stop->opcode, stop->arg, stop->flags,
+				stop->resp[0], stop->resp[1], stop->resp[2],
+				stop->resp[2], stop->error);
+	}
+
+	spin_unlock_irq(&host->mmc->lock);
+
+	return 0;
+}
+
+static int atmci_req_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, atmci_req_show, inode->i_private);
+}
+
+static const struct file_operations atmci_req_fops = {
+	.owner		= THIS_MODULE,
+	.open		= atmci_req_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void atmci_show_status_reg(struct seq_file *s,
+		const char *regname, u32 value)
+{
+	static const char	*sr_bit[] = {
+		[0]	= "CMDRDY",
+		[1]	= "RXRDY",
+		[2]	= "TXRDY",
+		[3]	= "BLKE",
+		[4]	= "DTIP",
+		[5]	= "NOTBUSY",
+		[8]	= "SDIOIRQA",
+		[9]	= "SDIOIRQB",
+		[16]	= "RINDE",
+		[17]	= "RDIRE",
+		[18]	= "RCRCE",
+		[19]	= "RENDE",
+		[20]	= "RTOE",
+		[21]	= "DCRCE",
+		[22]	= "DTOE",
+		[30]	= "OVRE",
+		[31]	= "UNRE",
+	};
+	unsigned int		i;
+
+	seq_printf(s, "%s:\t0x%08x", regname, value);
+	for (i = 0; i < ARRAY_SIZE(sr_bit); i++) {
+		if (value & (1 << i)) {
+			if (sr_bit[i])
+				seq_printf(s, " %s", sr_bit[i]);
+			else
+				seq_puts(s, " UNKNOWN");
+		}
+	}
+	seq_putc(s, '\n');
+}
+
+static int atmci_regs_show(struct seq_file *s, void *v)
+{
+	struct atmel_mci	*host = s->private;
+	u32			*buf;
+
+	buf = kmalloc(MCI_REGS_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	/* Grab a more or less consistent snapshot */
+	spin_lock_irq(&host->mmc->lock);
+	memcpy_fromio(buf, host->regs, MCI_REGS_SIZE);
+	spin_unlock_irq(&host->mmc->lock);
+
+	seq_printf(s, "MR:\t0x%08x%s%s CLKDIV=%u\n",
+			buf[MCI_MR / 4],
+			buf[MCI_MR / 4] & MCI_MR_RDPROOF ? " RDPROOF" : "",
+			buf[MCI_MR / 4] & MCI_MR_WRPROOF ? " WRPROOF" : "",
+			buf[MCI_MR / 4] & 0xff);
+	seq_printf(s, "DTOR:\t0x%08x\n", buf[MCI_DTOR / 4]);
+	seq_printf(s, "SDCR:\t0x%08x\n", buf[MCI_SDCR / 4]);
+	seq_printf(s, "ARGR:\t0x%08x\n", buf[MCI_ARGR / 4]);
+	seq_printf(s, "BLKR:\t0x%08x BCNT=%u BLKLEN=%u\n",
+			buf[MCI_BLKR / 4],
+			buf[MCI_BLKR / 4] & 0xffff,
+			(buf[MCI_BLKR / 4] >> 16) & 0xffff);
+
+	/* Don't read RSPR and RDR; it will consume the data there */
+
+	atmci_show_status_reg(s, "SR", buf[MCI_SR / 4]);
+	atmci_show_status_reg(s, "IMR", buf[MCI_IMR / 4]);
+
+	return 0;
+}
+
+static int atmci_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, atmci_regs_show, inode->i_private);
+}
+
+static const struct file_operations atmci_regs_fops = {
+	.owner		= THIS_MODULE,
+	.open		= atmci_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void atmci_init_debugfs(struct atmel_mci *host)
+{
+	struct mmc_host	*mmc;
+	struct dentry	*root;
+	struct dentry	*node;
+	struct resource	*res;
+
+	mmc = host->mmc;
+	root = mmc->debugfs_root;
+	if (!root)
+		return;
+
+	node = debugfs_create_file("regs", S_IRUSR, root, host,
+			&atmci_regs_fops);
+	if (IS_ERR(node))
+		return;
+	if (!node)
+		goto err;
+
+	res = platform_get_resource(host->pdev, IORESOURCE_MEM, 0);
+	node->d_inode->i_size = res->end - res->start + 1;
+
+	node = debugfs_create_file("req", S_IRUSR, root, host, &atmci_req_fops);
+	if (!node)
+		goto err;
+
+	node = debugfs_create_x32("pending_events", S_IRUSR, root,
+				     (u32 *)&host->pending_events);
+	if (!node)
+		goto err;
+
+	node = debugfs_create_x32("completed_events", S_IRUSR, root,
+				     (u32 *)&host->completed_events);
+	if (!node)
+		goto err;
+
+	return;
+
+err:
+	dev_err(&host->pdev->dev,
+		"failed to initialize debugfs for controller\n");
+}
 
 static void atmci_enable(struct atmel_mci *host)
 {
@@ -905,6 +1090,8 @@ static int __init atmci_probe(struct platform_device *pdev)
 			"Atmel MCI controller at 0x%08lx irq %d\n",
 			host->mapbase, irq);
 
+	atmci_init_debugfs(host);
+
 	return 0;
 
 err_request_irq:
@@ -923,6 +1110,8 @@ static int __exit atmci_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 
 	if (host) {
+		/* Debugfs stuff is cleaned up by mmc core */
+
 		if (host->detect_pin >= 0) {
 			int pin = host->detect_pin;
 
diff --git a/drivers/mmc/host/imxmmc.c b/drivers/mmc/host/imxmmc.c
index 5e880c0f134..f61406da65d 100644
--- a/drivers/mmc/host/imxmmc.c
+++ b/drivers/mmc/host/imxmmc.c
@@ -26,12 +26,6 @@
  *
  */
 
-#ifdef CONFIG_MMC_DEBUG
-#define DEBUG
-#else
-#undef  DEBUG
-#endif
-
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/ioport.h>
@@ -907,31 +901,12 @@ static const struct mmc_host_ops imxmci_ops = {
 	.get_ro		= imxmci_get_ro,
 };
 
-static struct resource *platform_device_resource(struct platform_device *dev, unsigned int mask, int nr)
-{
-	int i;
-
-	for (i = 0; i < dev->num_resources; i++)
-		if (dev->resource[i].flags == mask && nr-- == 0)
-			return &dev->resource[i];
-	return NULL;
-}
-
-static int platform_device_irq(struct platform_device *dev, int nr)
-{
-	int i;
-
-	for (i = 0; i < dev->num_resources; i++)
-		if (dev->resource[i].flags == IORESOURCE_IRQ && nr-- == 0)
-			return dev->resource[i].start;
-	return NO_IRQ;
-}
-
 static void imxmci_check_status(unsigned long data)
 {
 	struct imxmci_host *host = (struct imxmci_host *)data;
 
-	if( host->pdata->card_present(mmc_dev(host->mmc)) != host->present ) {
+	if (host->pdata && host->pdata->card_present &&
+	    host->pdata->card_present(mmc_dev(host->mmc)) != host->present) {
 		host->present ^= 1;
 		dev_info(mmc_dev(host->mmc), "card %s\n",
 		      host->present ? "inserted" : "removed");
@@ -962,13 +937,12 @@ static int imxmci_probe(struct platform_device *pdev)
 
 	printk(KERN_INFO "i.MX mmc driver\n");
 
-	r = platform_device_resource(pdev, IORESOURCE_MEM, 0);
-	irq = platform_device_irq(pdev, 0);
-	if (!r || irq == NO_IRQ)
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!r || irq < 0)
 		return -ENXIO;
 
-	r = request_mem_region(r->start, 0x100, "IMXMCI");
-	if (!r)
+	if (!request_mem_region(r->start, 0x100, pdev->name))
 		return -EBUSY;
 
 	mmc = mmc_alloc_host(sizeof(struct imxmci_host), &pdev->dev);
@@ -995,6 +969,8 @@ static int imxmci_probe(struct platform_device *pdev)
 	host->mmc = mmc;
 	host->dma_allocated = 0;
 	host->pdata = pdev->dev.platform_data;
+	if (!host->pdata)
+		dev_warn(&pdev->dev, "No platform data provided!\n");
 
 	spin_lock_init(&host->lock);
 	host->res = r;
@@ -1047,7 +1023,11 @@ static int imxmci_probe(struct platform_device *pdev)
 	if (ret)
 		goto out;
 
-	host->present = host->pdata->card_present(mmc_dev(mmc));
+	if (host->pdata && host->pdata->card_present)
+		host->present = host->pdata->card_present(mmc_dev(mmc));
+	else	/* if there is no way to detect assume that card is present */
+		host->present = 1;
+
 	init_timer(&host->timer);
 	host->timer.data = (unsigned long)host;
 	host->timer.function = imxmci_check_status;
@@ -1073,7 +1053,7 @@ out:
 	}
 	if (mmc)
 		mmc_free_host(mmc);
-	release_resource(r);
+	release_mem_region(r->start, 0x100);
 	return ret;
 }
 
@@ -1102,7 +1082,7 @@ static int imxmci_remove(struct platform_device *pdev)
 		clk_disable(host->clk);
 		clk_put(host->clk);
 
-		release_resource(host->res);
+		release_mem_region(host->res->start, 0x100);
 
 		mmc_free_host(mmc);
 	}
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index 41cc63360e4..7503b81374e 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1076,6 +1076,7 @@ static void mmc_spi_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		 */
 		if (canpower && ios->power_mode == MMC_POWER_OFF) {
 			int mres;
+			u8 nullbyte = 0;
 
 			host->spi->mode &= ~(SPI_CPOL|SPI_CPHA);
 			mres = spi_setup(host->spi);
@@ -1083,7 +1084,7 @@ static void mmc_spi_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 				dev_dbg(&host->spi->dev,
 					"switch to SPI mode 0 failed\n");
 
-			if (spi_w8r8(host->spi, 0x00) < 0)
+			if (spi_write(host->spi, &nullbyte, 1) < 0)
 				dev_dbg(&host->spi->dev,
 					"put spi signals to low failed\n");
 
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index c3a5db72ddd..5f95e10229b 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -337,7 +337,7 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 
 	host->align_addr = dma_map_single(mmc_dev(host->mmc),
 		host->align_buffer, 128 * 4, direction);
-	if (dma_mapping_error(host->align_addr))
+	if (dma_mapping_error(mmc_dev(host->mmc), host->align_addr))
 		goto fail;
 	BUG_ON(host->align_addr & 0x3);
 
@@ -439,7 +439,7 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 
 	host->adma_addr = dma_map_single(mmc_dev(host->mmc),
 		host->adma_desc, (128 * 2 + 1) * 4, DMA_TO_DEVICE);
-	if (dma_mapping_error(host->align_addr))
+	if (dma_mapping_error(mmc_dev(host->mmc), host->align_addr))
 		goto unmap_entries;
 	BUG_ON(host->adma_addr & 0x3);
 
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index a06bf8b8934..e354faee5df 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -9,6 +9,8 @@
  * your option) any later version.
  */
 
+#include <linux/scatterlist.h>
+
 /*
  * Controller registers
  */
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index eed06d068fd..14f11f8b9e5 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -1,5 +1,3 @@
-# $Id: Kconfig,v 1.11 2005/11/07 11:14:19 gleixner Exp $
-
 menuconfig MTD
 	tristate "Memory Technology Device (MTD) support"
 	depends on HAS_IOMEM
diff --git a/drivers/mtd/afs.c b/drivers/mtd/afs.c
index 52d51eb91c1..d072ca5be68 100644
--- a/drivers/mtd/afs.c
+++ b/drivers/mtd/afs.c
@@ -21,8 +21,6 @@
    This is access code for flashes using ARM's flash partitioning
    standards.
 
-   $Id: afs.c,v 1.15 2005/11/07 11:14:19 gleixner Exp $
-
 ======================================================================*/
 
 #include <linux/module.h>
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index fcd1aeccdf9..5f1b472137a 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -4,8 +4,6 @@
  *
  * (C) 2000 Red Hat. GPL'd
  *
- * $Id: cfi_cmdset_0001.c,v 1.186 2005/11/23 22:07:52 nico Exp $
- *
  *
  * 10/10/2000	Nicolas Pitre <nico@cam.org>
  * 	- completely revamped method functions so they are aware and
@@ -50,6 +48,8 @@
 #define I82802AC	0x00ac
 #define MANUFACTURER_ST         0x0020
 #define M50LPW080       0x002F
+#define M50FLW080A	0x0080
+#define M50FLW080B	0x0081
 #define AT49BV640D	0x02de
 
 static int cfi_intelext_read (struct mtd_info *, loff_t, size_t, size_t *, u_char *);
@@ -204,7 +204,7 @@ static void fixup_intel_strataflash(struct mtd_info *mtd, void* param)
 {
 	struct map_info *map = mtd->priv;
 	struct cfi_private *cfi = map->fldrv_priv;
-	struct cfi_pri_amdstd *extp = cfi->cmdset_priv;
+	struct cfi_pri_intelext *extp = cfi->cmdset_priv;
 
 	printk(KERN_WARNING "cfi_cmdset_0001: Suspend "
 	                    "erase on write disabled.\n");
@@ -301,6 +301,8 @@ static struct cfi_fixup jedec_fixup_table[] = {
 	{ MANUFACTURER_INTEL, I82802AB,   fixup_use_fwh_lock, NULL, },
 	{ MANUFACTURER_INTEL, I82802AC,   fixup_use_fwh_lock, NULL, },
 	{ MANUFACTURER_ST,    M50LPW080,  fixup_use_fwh_lock, NULL, },
+	{ MANUFACTURER_ST,    M50FLW080A, fixup_use_fwh_lock, NULL, },
+	{ MANUFACTURER_ST,    M50FLW080B, fixup_use_fwh_lock, NULL, },
 	{ 0, 0, NULL, NULL }
 };
 static struct cfi_fixup fixup_table[] = {
@@ -1147,7 +1149,7 @@ static int inval_cache_and_wait_for_operation(
 	struct cfi_private *cfi = map->fldrv_priv;
 	map_word status, status_OK = CMD(0x80);
 	int chip_state = chip->state;
-	unsigned int timeo, sleep_time;
+	unsigned int timeo, sleep_time, reset_timeo;
 
 	spin_unlock(chip->mutex);
 	if (inval_len)
@@ -1158,6 +1160,7 @@ static int inval_cache_and_wait_for_operation(
 	timeo = chip_op_time * 8;
 	if (!timeo)
 		timeo = 500000;
+	reset_timeo = timeo;
 	sleep_time = chip_op_time / 2;
 
 	for (;;) {
@@ -1199,6 +1202,12 @@ static int inval_cache_and_wait_for_operation(
 			remove_wait_queue(&chip->wq, &wait);
 			spin_lock(chip->mutex);
 		}
+		if (chip->erase_suspended || chip->write_suspended)  {
+			/* Suspend has occured while sleep: reset timeout */
+			timeo = reset_timeo;
+			chip->erase_suspended = 0;
+			chip->write_suspended = 0;
+		}
 	}
 
 	/* Done and happy. */
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index f7fcc638953..a972cc6be43 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -16,9 +16,6 @@
  * Occasionally maintained by Thayne Harbaugh tharbaugh at lnxi dot com
  *
  * This code is GPL
- *
- * $Id: cfi_cmdset_0002.c,v 1.122 2005/11/07 11:14:22 gleixner Exp $
- *
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/chips/cfi_cmdset_0020.c b/drivers/mtd/chips/cfi_cmdset_0020.c
index 1b720cc571f..d4714dd9f7a 100644
--- a/drivers/mtd/chips/cfi_cmdset_0020.c
+++ b/drivers/mtd/chips/cfi_cmdset_0020.c
@@ -4,8 +4,6 @@
  *
  * (C) 2000 Red Hat. GPL'd
  *
- * $Id: cfi_cmdset_0020.c,v 1.22 2005/11/07 11:14:22 gleixner Exp $
- *
  * 10/10/2000	Nicolas Pitre <nico@cam.org>
  * 	- completely revamped method functions so they are aware and
  * 	  independent of the flash geometry (buswidth, interleave, etc.)
diff --git a/drivers/mtd/chips/cfi_probe.c b/drivers/mtd/chips/cfi_probe.c
index a4463a91ce3..c418e92e1d9 100644
--- a/drivers/mtd/chips/cfi_probe.c
+++ b/drivers/mtd/chips/cfi_probe.c
@@ -1,7 +1,6 @@
 /*
    Common Flash Interface probe code.
    (C) 2000 Red Hat. GPL'd.
-   $Id: cfi_probe.c,v 1.86 2005/11/29 14:48:31 gleixner Exp $
 */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/chips/cfi_util.c b/drivers/mtd/chips/cfi_util.c
index 72e0022a47b..0ee45701801 100644
--- a/drivers/mtd/chips/cfi_util.c
+++ b/drivers/mtd/chips/cfi_util.c
@@ -6,9 +6,6 @@
  * Copyright (C) 2003 STMicroelectronics Limited
  *
  * This code is covered by the GPL.
- *
- * $Id: cfi_util.c,v 1.10 2005/11/07 11:14:23 gleixner Exp $
- *
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/chips/chipreg.c b/drivers/mtd/chips/chipreg.c
index 2174c97549f..c8576096822 100644
--- a/drivers/mtd/chips/chipreg.c
+++ b/drivers/mtd/chips/chipreg.c
@@ -1,6 +1,4 @@
 /*
- * $Id: chipreg.c,v 1.17 2004/11/16 18:29:00 dwmw2 Exp $
- *
  * Registration for chip drivers
  *
  */
diff --git a/drivers/mtd/chips/gen_probe.c b/drivers/mtd/chips/gen_probe.c
index d338b8c9278..f061885b281 100644
--- a/drivers/mtd/chips/gen_probe.c
+++ b/drivers/mtd/chips/gen_probe.c
@@ -2,7 +2,6 @@
  * Routines common to all CFI-type probes.
  * (C) 2001-2003 Red Hat, Inc.
  * GPL'd
- * $Id: gen_probe.c,v 1.24 2005/11/07 11:14:23 gleixner Exp $
  */
 
 #include <linux/kernel.h>
@@ -71,8 +70,8 @@ static struct cfi_private *genprobe_ident_chips(struct map_info *map, struct chi
 	   interleave and device type, etc. */
 	if (!genprobe_new_chip(map, cp, &cfi)) {
 		/* The probe didn't like it */
-		printk(KERN_DEBUG "%s: Found no %s device at location zero\n",
-		       cp->name, map->name);
+		pr_debug("%s: Found no %s device at location zero\n",
+			 cp->name, map->name);
 		return NULL;
 	}
 
diff --git a/drivers/mtd/chips/jedec_probe.c b/drivers/mtd/chips/jedec_probe.c
index aa07575eb28..dbba5abf0db 100644
--- a/drivers/mtd/chips/jedec_probe.c
+++ b/drivers/mtd/chips/jedec_probe.c
@@ -1,7 +1,6 @@
 /*
    Common Flash Interface probe code.
    (C) 2000 Red Hat. GPL'd.
-   $Id: jedec_probe.c,v 1.66 2005/11/07 11:14:23 gleixner Exp $
    See JEDEC (http://www.jedec.org/) standard JESD21C (section 3.5)
    for the standard this probe goes back to.
 
@@ -26,6 +25,7 @@
 /* Manufacturers */
 #define MANUFACTURER_AMD	0x0001
 #define MANUFACTURER_ATMEL	0x001f
+#define MANUFACTURER_EON	0x001c
 #define MANUFACTURER_FUJITSU	0x0004
 #define MANUFACTURER_HYUNDAI	0x00AD
 #define MANUFACTURER_INTEL	0x0089
@@ -37,6 +37,7 @@
 #define MANUFACTURER_ST		0x0020
 #define MANUFACTURER_TOSHIBA	0x0098
 #define MANUFACTURER_WINBOND	0x00da
+#define CONTINUATION_CODE	0x007f
 
 
 /* AMD */
@@ -58,6 +59,8 @@
 #define AM29LV040B	0x004F
 #define AM29F032B	0x0041
 #define AM29F002T	0x00B0
+#define AM29SL800DB	0x226B
+#define AM29SL800DT	0x22EA
 
 /* Atmel */
 #define AT49BV512	0x0003
@@ -67,6 +70,10 @@
 #define AT49BV32X	0x00C8
 #define AT49BV32XT	0x00C9
 
+/* Eon */
+#define EN29SL800BB	0x226B
+#define EN29SL800BT	0x22EA
+
 /* Fujitsu */
 #define MBM29F040C	0x00A4
 #define MBM29F800BA	0x2258
@@ -141,6 +148,8 @@
 #define M50FW080	0x002D
 #define M50FW016	0x002E
 #define M50LPW080       0x002F
+#define M50FLW080A	0x0080
+#define M50FLW080B	0x0081
 
 /* SST */
 #define SST29EE020	0x0010
@@ -191,6 +200,7 @@ enum uaddr {
 	MTD_UADDR_0x0555_0x0AAA,
 	MTD_UADDR_0x5555_0x2AAA,
 	MTD_UADDR_0x0AAA_0x0555,
+	MTD_UADDR_0xAAAA_0x5555,
 	MTD_UADDR_DONT_CARE,		/* Requires an arbitrary address */
 	MTD_UADDR_UNNECESSARY,		/* Does not require any address */
 };
@@ -238,6 +248,11 @@ static const struct unlock_addr  unlock_addrs[] = {
 		.addr2 = 0x0555
 	},
 
+	[MTD_UADDR_0xAAAA_0x5555] = {
+		.addr1 = 0xaaaa,
+		.addr2 = 0x5555
+	},
+
 	[MTD_UADDR_DONT_CARE] = {
 		.addr1 = 0x0000,      /* Doesn't matter which address */
 		.addr2 = 0x0000       /* is used - must be last entry */
@@ -522,6 +537,36 @@ static const struct amd_flash_info jedec_table[] = {
 			ERASEINFO(0x04000,1),
 		}
 	}, {
+		.mfr_id		= MANUFACTURER_AMD,
+		.dev_id		= AM29SL800DT,
+		.name		= "AMD AM29SL800DT",
+		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_0x0AAA_0x0555,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_AMD_STD,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x10000,15),
+			ERASEINFO(0x08000,1),
+			ERASEINFO(0x02000,2),
+			ERASEINFO(0x04000,1),
+		}
+	}, {
+		.mfr_id		= MANUFACTURER_AMD,
+		.dev_id		= AM29SL800DB,
+		.name		= "AMD AM29SL800DB",
+		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_0x0AAA_0x0555,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_AMD_STD,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x04000,1),
+			ERASEINFO(0x02000,2),
+			ERASEINFO(0x08000,1),
+			ERASEINFO(0x10000,15),
+		}
+	}, {
 		.mfr_id		= MANUFACTURER_ATMEL,
 		.dev_id		= AT49BV512,
 		.name		= "Atmel AT49BV512",
@@ -599,6 +644,36 @@ static const struct amd_flash_info jedec_table[] = {
 			ERASEINFO(0x02000,8)
 		}
 	}, {
+		.mfr_id		= MANUFACTURER_EON,
+		.dev_id		= EN29SL800BT,
+		.name		= "Eon EN29SL800BT",
+		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_0x0AAA_0x0555,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_AMD_STD,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x10000,15),
+			ERASEINFO(0x08000,1),
+			ERASEINFO(0x02000,2),
+			ERASEINFO(0x04000,1),
+		}
+	}, {
+		.mfr_id		= MANUFACTURER_EON,
+		.dev_id		= EN29SL800BB,
+		.name		= "Eon EN29SL800BB",
+		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_0x0AAA_0x0555,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_AMD_STD,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x04000,1),
+			ERASEINFO(0x02000,2),
+			ERASEINFO(0x08000,1),
+			ERASEINFO(0x10000,15),
+		}
+	}, {
 		.mfr_id		= MANUFACTURER_FUJITSU,
 		.dev_id		= MBM29F040C,
 		.name		= "Fujitsu MBM29F040C",
@@ -1392,8 +1467,8 @@ static const struct amd_flash_info jedec_table[] = {
 		.mfr_id		= MANUFACTURER_SST,     /* should be CFI */
 		.dev_id		= SST39LF160,
 		.name		= "SST 39LF160",
-		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
-		.uaddr		= MTD_UADDR_0x5555_0x2AAA,	/* ???? */
+		.devtypes	= CFI_DEVICETYPE_X16,
+		.uaddr		= MTD_UADDR_0xAAAA_0x5555,
 		.dev_size	= SIZE_2MiB,
 		.cmd_set	= P_ID_AMD_STD,
 		.nr_regions	= 2,
@@ -1405,8 +1480,8 @@ static const struct amd_flash_info jedec_table[] = {
 		.mfr_id		= MANUFACTURER_SST,     /* should be CFI */
 		.dev_id		= SST39VF1601,
 		.name		= "SST 39VF1601",
-		.devtypes	= CFI_DEVICETYPE_X16|CFI_DEVICETYPE_X8,
-		.uaddr		= MTD_UADDR_0x5555_0x2AAA,	/* ???? */
+		.devtypes	= CFI_DEVICETYPE_X16,
+		.uaddr		= MTD_UADDR_0xAAAA_0x5555,
 		.dev_size	= SIZE_2MiB,
 		.cmd_set	= P_ID_AMD_STD,
 		.nr_regions	= 2,
@@ -1590,6 +1665,36 @@ static const struct amd_flash_info jedec_table[] = {
 		.nr_regions	= 1,
 		.regions	= {
 			ERASEINFO(0x10000,16),
+		},
+	}, {
+		.mfr_id		= MANUFACTURER_ST,
+		.dev_id		= M50FLW080A,
+		.name		= "ST M50FLW080A",
+		.devtypes	= CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_UNNECESSARY,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_INTEL_EXT,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x1000,16),
+			ERASEINFO(0x10000,13),
+			ERASEINFO(0x1000,16),
+			ERASEINFO(0x1000,16),
+		}
+	}, {
+		.mfr_id		= MANUFACTURER_ST,
+		.dev_id		= M50FLW080B,
+		.name		= "ST M50FLW080B",
+		.devtypes	= CFI_DEVICETYPE_X8,
+		.uaddr		= MTD_UADDR_UNNECESSARY,
+		.dev_size	= SIZE_1MiB,
+		.cmd_set	= P_ID_INTEL_EXT,
+		.nr_regions	= 4,
+		.regions	= {
+			ERASEINFO(0x1000,16),
+			ERASEINFO(0x1000,16),
+			ERASEINFO(0x10000,13),
+			ERASEINFO(0x1000,16),
 		}
 	}, {
 		.mfr_id		= MANUFACTURER_TOSHIBA,
@@ -1696,9 +1801,21 @@ static inline u32 jedec_read_mfr(struct map_info *map, uint32_t base,
 {
 	map_word result;
 	unsigned long mask;
-	u32 ofs = cfi_build_cmd_addr(0, cfi_interleave(cfi), cfi->device_type);
-	mask = (1 << (cfi->device_type * 8)) -1;
-	result = map_read(map, base + ofs);
+	int bank = 0;
+
+	/* According to JEDEC "Standard Manufacturer's Identification Code"
+	 * (http://www.jedec.org/download/search/jep106W.pdf)
+	 * several first banks can contain 0x7f instead of actual ID
+	 */
+	do {
+		uint32_t ofs = cfi_build_cmd_addr(0 + (bank << 8),
+						  cfi_interleave(cfi),
+						  cfi->device_type);
+		mask = (1 << (cfi->device_type * 8)) - 1;
+		result = map_read(map, base + ofs);
+		bank++;
+	} while ((result.x[0] & mask) == CONTINUATION_CODE);
+
 	return result.x[0] & mask;
 }
 
diff --git a/drivers/mtd/chips/map_absent.c b/drivers/mtd/chips/map_absent.c
index fc478c0f93f..494d30d0631 100644
--- a/drivers/mtd/chips/map_absent.c
+++ b/drivers/mtd/chips/map_absent.c
@@ -1,7 +1,6 @@
 /*
  * Common code to handle absent "placeholder" devices
  * Copyright 2001 Resilience Corporation <ebrower@resilience.com>
- * $Id: map_absent.c,v 1.6 2005/11/07 11:14:23 gleixner Exp $
  *
  * This map driver is used to allocate "placeholder" MTD
  * devices on systems that have socketed/removable media.
diff --git a/drivers/mtd/chips/map_ram.c b/drivers/mtd/chips/map_ram.c
index 5cb6d526366..072dd8abf33 100644
--- a/drivers/mtd/chips/map_ram.c
+++ b/drivers/mtd/chips/map_ram.c
@@ -1,7 +1,6 @@
 /*
  * Common code to handle map devices which are simple RAM
  * (C) 2000 Red Hat. GPL'd.
- * $Id: map_ram.c,v 1.22 2005/01/05 18:05:12 dwmw2 Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/chips/map_rom.c b/drivers/mtd/chips/map_rom.c
index cb27f855074..821d0ed6bae 100644
--- a/drivers/mtd/chips/map_rom.c
+++ b/drivers/mtd/chips/map_rom.c
@@ -1,7 +1,6 @@
 /*
  * Common code to handle map devices which are simple ROM
  * (C) 2000 Red Hat. GPL'd.
- * $Id: map_rom.c,v 1.23 2005/01/05 18:05:12 dwmw2 Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/cmdlinepart.c b/drivers/mtd/cmdlinepart.c
index e472a0e9de9..71bc07f149b 100644
--- a/drivers/mtd/cmdlinepart.c
+++ b/drivers/mtd/cmdlinepart.c
@@ -1,6 +1,4 @@
 /*
- * $Id: cmdlinepart.c,v 1.19 2005/11/07 11:14:19 gleixner Exp $
- *
  * Read flash partition table from command line
  *
  * Copyright 2002 SYSGO Real-Time Solutions GmbH
@@ -308,7 +306,7 @@ static int parse_cmdline_partitions(struct mtd_info *master,
 	unsigned long offset;
 	int i;
 	struct cmdline_mtd_partition *part;
-	char *mtd_id = master->name;
+	const char *mtd_id = master->name;
 
 	/* parse command line */
 	if (!cmdline_parsed)
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 35ed1103dbb..9c613f06623 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -1,5 +1,4 @@
 # drivers/mtd/maps/Kconfig
-# $Id: Kconfig,v 1.18 2005/11/07 11:14:24 gleixner Exp $
 
 menu "Self-contained MTD device drivers"
 	depends on MTD!=n
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index 0f788d5c4bf..0993d5cf392 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -1,7 +1,6 @@
 #
 # linux/drivers/devices/Makefile
 #
-# $Id: Makefile.common,v 1.7 2004/12/22 17:51:15 joern Exp $
 
 obj-$(CONFIG_MTD_DOC2000)	+= doc2000.o
 obj-$(CONFIG_MTD_DOC2001)	+= doc2001.o
diff --git a/drivers/mtd/devices/block2mtd.c b/drivers/mtd/devices/block2mtd.c
index 7b72a1b3611..91fbba76763 100644
--- a/drivers/mtd/devices/block2mtd.c
+++ b/drivers/mtd/devices/block2mtd.c
@@ -1,6 +1,4 @@
 /*
- * $Id: block2mtd.c,v 1.30 2005/11/29 14:48:32 gleixner Exp $
- *
  * block2mtd.c - create an mtd from a block device
  *
  * Copyright (C) 2001,2002	Simon Evans <spse@secret.org.uk>
@@ -20,9 +18,6 @@
 #include <linux/mutex.h>
 #include <linux/mount.h>
 
-#define VERSION "$Revision: 1.30 $"
-
-
 #define ERROR(fmt, args...) printk(KERN_ERR "block2mtd: " fmt "\n" , ## args)
 #define INFO(fmt, args...) printk(KERN_INFO "block2mtd: " fmt "\n" , ## args)
 
@@ -453,7 +448,6 @@ MODULE_PARM_DESC(block2mtd, "Device to use. \"block2mtd=<dev>[,<erasesize>]\"");
 static int __init block2mtd_init(void)
 {
 	int ret = 0;
-	INFO("version " VERSION);
 
 #ifndef MODULE
 	if (strlen(block2mtd_paramline))
diff --git a/drivers/mtd/devices/doc2000.c b/drivers/mtd/devices/doc2000.c
index 846989f292e..50de839c77a 100644
--- a/drivers/mtd/devices/doc2000.c
+++ b/drivers/mtd/devices/doc2000.c
@@ -3,8 +3,6 @@
  * Linux driver for Disk-On-Chip 2000 and Millennium
  * (c) 1999 Machine Vision Holdings, Inc.
  * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
- *
- * $Id: doc2000.c,v 1.67 2005/11/07 11:14:24 gleixner Exp $
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/mtd/devices/doc2001.c b/drivers/mtd/devices/doc2001.c
index 6413efc045e..e32c568c114 100644
--- a/drivers/mtd/devices/doc2001.c
+++ b/drivers/mtd/devices/doc2001.c
@@ -3,8 +3,6 @@
  * Linux driver for Disk-On-Chip Millennium
  * (c) 1999 Machine Vision Holdings, Inc.
  * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
- *
- * $Id: doc2001.c,v 1.49 2005/11/07 11:14:24 gleixner Exp $
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/mtd/devices/doc2001plus.c b/drivers/mtd/devices/doc2001plus.c
index 83be3461658..d853f891b58 100644
--- a/drivers/mtd/devices/doc2001plus.c
+++ b/drivers/mtd/devices/doc2001plus.c
@@ -6,8 +6,6 @@
  * (c) 1999 Machine Vision Holdings, Inc.
  * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
  *
- * $Id: doc2001plus.c,v 1.14 2005/11/07 11:14:24 gleixner Exp $
- *
  * Released under GPL
  */
 
diff --git a/drivers/mtd/devices/docecc.c b/drivers/mtd/devices/docecc.c
index fd8a8daba3a..874e51b110a 100644
--- a/drivers/mtd/devices/docecc.c
+++ b/drivers/mtd/devices/docecc.c
@@ -7,8 +7,6 @@
  * Author: Fabrice Bellard (fabrice.bellard@netgem.com)
  * Copyright (C) 2000 Netgem S.A.
  *
- * $Id: docecc.c,v 1.7 2005/11/07 11:14:25 gleixner Exp $
- *
  * This program is free software; 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
diff --git a/drivers/mtd/devices/docprobe.c b/drivers/mtd/devices/docprobe.c
index d8cc94ec4e5..6e62922942b 100644
--- a/drivers/mtd/devices/docprobe.c
+++ b/drivers/mtd/devices/docprobe.c
@@ -4,9 +4,6 @@
 /* (C) 1999 Machine Vision Holdings, Inc.			*/
 /* (C) 1999-2003 David Woodhouse <dwmw2@infradead.org>		*/
 
-/* $Id: docprobe.c,v 1.46 2005/11/07 11:14:25 gleixner Exp $	*/
-
-
 
 /* DOC_PASSIVE_PROBE:
    In order to ensure that the BIOS checksum is correct at boot time, and
@@ -79,8 +76,6 @@ static unsigned long __initdata doc_locations[] = {
 	0xe0000, 0xe2000, 0xe4000, 0xe6000,
 	0xe8000, 0xea000, 0xec000, 0xee000,
 #endif /*  CONFIG_MTD_DOCPROBE_HIGH */
-#elif defined(__PPC__)
-	0xe4000000,
 #else
 #warning Unknown architecture for DiskOnChip. No default probe locations defined
 #endif
diff --git a/drivers/mtd/devices/lart.c b/drivers/mtd/devices/lart.c
index 1d324e5c412..f4bda4cee49 100644
--- a/drivers/mtd/devices/lart.c
+++ b/drivers/mtd/devices/lart.c
@@ -2,8 +2,6 @@
 /*
  * MTD driver for the 28F160F3 Flash Memory (non-CFI) on LART.
  *
- * $Id: lart.c,v 1.9 2005/11/07 11:14:25 gleixner Exp $
- *
  * Author: Abraham vd Merwe <abraham@2d3d.co.za>
  *
  * Copyright (c) 2001, 2d3D, Inc.
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index b402269301f..b35c3333e21 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -33,6 +33,7 @@
 /* Flash opcodes. */
 #define	OPCODE_WREN		0x06	/* Write enable */
 #define	OPCODE_RDSR		0x05	/* Read status register */
+#define	OPCODE_WRSR		0x01	/* Write status register 1 byte */
 #define	OPCODE_NORM_READ	0x03	/* Read data bytes (low frequency) */
 #define	OPCODE_FAST_READ	0x0b	/* Read data bytes (high frequency) */
 #define	OPCODE_PP		0x02	/* Page program (up to 256 bytes) */
@@ -112,6 +113,17 @@ static int read_sr(struct m25p *flash)
 	return val;
 }
 
+/*
+ * Write status register 1 byte
+ * Returns negative if error occurred.
+ */
+static int write_sr(struct m25p *flash, u8 val)
+{
+	flash->command[0] = OPCODE_WRSR;
+	flash->command[1] = val;
+
+	return spi_write(flash->spi, flash->command, 2);
+}
 
 /*
  * Set write enable latch with Write Enable command.
@@ -589,6 +601,16 @@ static int __devinit m25p_probe(struct spi_device *spi)
 	mutex_init(&flash->lock);
 	dev_set_drvdata(&spi->dev, flash);
 
+	/*
+	 * Atmel serial flash tend to power up
+	 * with the software protection bits set
+	 */
+
+	if (info->jedec_id >> 16 == 0x1f) {
+		write_enable(flash);
+		write_sr(flash, 0);
+	}
+
 	if (data && data->name)
 		flash->mtd.name = data->name;
 	else
diff --git a/drivers/mtd/devices/ms02-nv.c b/drivers/mtd/devices/ms02-nv.c
index 9cff119a202..6a9a24a80a6 100644
--- a/drivers/mtd/devices/ms02-nv.c
+++ b/drivers/mtd/devices/ms02-nv.c
@@ -5,8 +5,6 @@
  *	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.
- *
- *	$Id: ms02-nv.c,v 1.11 2005/11/14 13:41:47 macro Exp $
  */
 
 #include <linux/init.h>
diff --git a/drivers/mtd/devices/ms02-nv.h b/drivers/mtd/devices/ms02-nv.h
index 8a6eef7cfee..04deafd3a77 100644
--- a/drivers/mtd/devices/ms02-nv.h
+++ b/drivers/mtd/devices/ms02-nv.h
@@ -9,8 +9,6 @@
  *	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.
- *
- *	$Id: ms02-nv.h,v 1.3 2003/08/19 09:25:36 dwmw2 Exp $
  */
 
 #include <linux/ioport.h>
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index b35e4813a3a..54e36bfc2c3 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -82,7 +82,7 @@
 
 
 struct dataflash {
-	u8			command[4];
+	uint8_t			command[4];
 	char			name[24];
 
 	unsigned		partitioned:1;
@@ -150,7 +150,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 	struct spi_transfer	x = { .tx_dma = 0, };
 	struct spi_message	msg;
 	unsigned		blocksize = priv->page_size << 3;
-	u8			*command;
+	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: erase addr=0x%x len 0x%x\n",
 			spi->dev.bus_id,
@@ -182,8 +182,8 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 		pageaddr = pageaddr << priv->page_offset;
 
 		command[0] = do_block ? OP_ERASE_BLOCK : OP_ERASE_PAGE;
-		command[1] = (u8)(pageaddr >> 16);
-		command[2] = (u8)(pageaddr >> 8);
+		command[1] = (uint8_t)(pageaddr >> 16);
+		command[2] = (uint8_t)(pageaddr >> 8);
 		command[3] = 0;
 
 		DEBUG(MTD_DEBUG_LEVEL3, "ERASE %s: (%x) %x %x %x [%i]\n",
@@ -234,7 +234,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct spi_transfer	x[2] = { { .tx_dma = 0, }, };
 	struct spi_message	msg;
 	unsigned int		addr;
-	u8			*command;
+	uint8_t			*command;
 	int			status;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: read 0x%x..0x%x\n",
@@ -274,9 +274,9 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	 * fewer "don't care" bytes.  Both buffers stay unchanged.
 	 */
 	command[0] = OP_READ_CONTINUOUS;
-	command[1] = (u8)(addr >> 16);
-	command[2] = (u8)(addr >> 8);
-	command[3] = (u8)(addr >> 0);
+	command[1] = (uint8_t)(addr >> 16);
+	command[2] = (uint8_t)(addr >> 8);
+	command[3] = (uint8_t)(addr >> 0);
 	/* plus 4 "don't care" bytes */
 
 	status = spi_sync(priv->spi, &msg);
@@ -311,7 +311,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 	size_t			remaining = len;
 	u_char			*writebuf = (u_char *) buf;
 	int			status = -EINVAL;
-	u8			*command;
+	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: write 0x%x..0x%x\n",
 		spi->dev.bus_id, (unsigned)to, (unsigned)(to + len));
@@ -487,7 +487,9 @@ add_dataflash(struct spi_device *spi, char *name,
 	device->write = dataflash_write;
 	device->priv = priv;
 
-	dev_info(&spi->dev, "%s (%d KBytes)\n", name, device->size/1024);
+	dev_info(&spi->dev, "%s (%d KBytes) pagesize %d bytes, "
+		"erasesize %d bytes\n", name, device->size/1024,
+		 pagesize, pagesize * 8);	/* 8 pages = 1 block */
 	dev_set_drvdata(&spi->dev, priv);
 
 	if (mtd_has_partitions()) {
@@ -521,7 +523,7 @@ add_dataflash(struct spi_device *spi, char *name,
  *
  *   Device      Density         ID code          #Pages PageSize  Offset
  *   AT45DB011B  1Mbit   (128K)  xx0011xx (0x0c)    512    264      9
- *   AT45DB021B  2Mbit   (256K)  xx0101xx (0x14)   1025    264      9
+ *   AT45DB021B  2Mbit   (256K)  xx0101xx (0x14)   1024    264      9
  *   AT45DB041B  4Mbit   (512K)  xx0111xx (0x1c)   2048    264      9
  *   AT45DB081B  8Mbit   (1M)    xx1001xx (0x24)   4096    264      9
  *   AT45DB0161B 16Mbit  (2M)    xx1011xx (0x2c)   4096    528     10
@@ -529,9 +531,114 @@ add_dataflash(struct spi_device *spi, char *name,
  *   AT45DB0642  64Mbit  (8M)    xx111xxx (0x3c)   8192   1056     11
  *   AT45DB1282  128Mbit (16M)   xx0100xx (0x10)  16384   1056     11
  */
+
+struct flash_info {
+	char		*name;
+
+	/* JEDEC id zero means "no ID" (most older chips); otherwise it has
+	 * a high byte of zero plus three data bytes: the manufacturer id,
+	 * then a two byte device id.
+	 */
+	uint32_t	jedec_id;
+
+	/* The size listed here is what works with OPCODE_SE, which isn't
+	 * necessarily called a "sector" by the vendor.
+	 */
+	unsigned	nr_pages;
+	uint16_t	pagesize;
+	uint16_t	pageoffset;
+
+	uint16_t	flags;
+#define	SUP_POW2PS	0x02
+#define	IS_POW2PS	0x01
+};
+
+static struct flash_info __devinitdata dataflash_data [] = {
+
+	{ "at45db011d",  0x1f2200, 512, 264, 9, SUP_POW2PS},
+	{ "at45db011d",  0x1f2200, 512, 256, 8, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db021d",  0x1f2300, 1024, 264, 9, SUP_POW2PS},
+	{ "at45db021d",  0x1f2300, 1024, 256, 8, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db041d",  0x1f2400, 2048, 264, 9, SUP_POW2PS},
+	{ "at45db041d",  0x1f2400, 2048, 256, 8, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db081d",  0x1f2500, 4096, 264, 9, SUP_POW2PS},
+	{ "at45db081d",  0x1f2500, 4096, 256, 8, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db161d",  0x1f2600, 4096, 528, 10, SUP_POW2PS},
+	{ "at45db161d",  0x1f2600, 4096, 512, 9, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db321c",  0x1f2700, 8192, 528, 10, },
+
+	{ "at45db321d",  0x1f2701, 8192, 528, 10, SUP_POW2PS},
+	{ "at45db321d",  0x1f2701, 8192, 512, 9, SUP_POW2PS | IS_POW2PS},
+
+	{ "at45db641d",  0x1f2800, 8192, 1056, 11, SUP_POW2PS},
+	{ "at45db641d",  0x1f2800, 8192, 1024, 10, SUP_POW2PS | IS_POW2PS},
+};
+
+static struct flash_info *__devinit jedec_probe(struct spi_device *spi)
+{
+	int			tmp;
+	uint8_t			code = OP_READ_ID;
+	uint8_t			id[3];
+	uint32_t		jedec;
+	struct flash_info	*info;
+	int status;
+
+
+	/* JEDEC also defines an optional "extended device information"
+	 * string for after vendor-specific data, after the three bytes
+	 * we use here.  Supporting some chips might require using it.
+	 */
+	tmp = spi_write_then_read(spi, &code, 1, id, 3);
+	if (tmp < 0) {
+		DEBUG(MTD_DEBUG_LEVEL0, "%s: error %d reading JEDEC ID\n",
+			spi->dev.bus_id, tmp);
+		return NULL;
+	}
+	jedec = id[0];
+	jedec = jedec << 8;
+	jedec |= id[1];
+	jedec = jedec << 8;
+	jedec |= id[2];
+
+	for (tmp = 0, info = dataflash_data;
+			tmp < ARRAY_SIZE(dataflash_data);
+			tmp++, info++) {
+		if (info->jedec_id == jedec) {
+			if (info->flags & SUP_POW2PS) {
+				status = dataflash_status(spi);
+				if (status & 0x1)
+					/* return power of 2 pagesize */
+					return ++info;
+				else
+					return info;
+			}
+		}
+	}
+	return NULL;
+}
+
 static int __devinit dataflash_probe(struct spi_device *spi)
 {
 	int status;
+	struct flash_info	*info;
+
+	/*
+	 * Try to detect dataflash by JEDEC ID.
+	 * If it succeeds we know we have either a C or D part.
+	 * D will support power of 2 pagesize option.
+	 */
+
+	info = jedec_probe(spi);
+
+	if (info != NULL)
+		return add_dataflash(spi, info->name, info->nr_pages,
+				 info->pagesize, info->pageoffset);
+
 
 	status = dataflash_status(spi);
 	if (status <= 0 || status == 0xff) {
@@ -551,16 +658,16 @@ static int __devinit dataflash_probe(struct spi_device *spi)
 		status = add_dataflash(spi, "AT45DB011B", 512, 264, 9);
 		break;
 	case 0x14:	/* 0 1 0 1 x x */
-		status = add_dataflash(spi, "AT45DB021B", 1025, 264, 9);
+		status = add_dataflash(spi, "AT45DB021B", 1024, 264, 9);
 		break;
 	case 0x1c:	/* 0 1 1 1 x x */
-		status = add_dataflash(spi, "AT45DB041x", 2048, 264, 9);
+		status = add_dataflash(spi, "AT45DB041B", 2048, 264, 9);
 		break;
 	case 0x24:	/* 1 0 0 1 x x */
 		status = add_dataflash(spi, "AT45DB081B", 4096, 264, 9);
 		break;
 	case 0x2c:	/* 1 0 1 1 x x */
-		status = add_dataflash(spi, "AT45DB161x", 4096, 528, 10);
+		status = add_dataflash(spi, "AT45DB161B", 4096, 528, 10);
 		break;
 	case 0x34:	/* 1 1 0 1 x x */
 		status = add_dataflash(spi, "AT45DB321x", 8192, 528, 10);
diff --git a/drivers/mtd/devices/mtdram.c b/drivers/mtd/devices/mtdram.c
index 0399be17862..3aaca88847d 100644
--- a/drivers/mtd/devices/mtdram.c
+++ b/drivers/mtd/devices/mtdram.c
@@ -1,6 +1,5 @@
 /*
  * mtdram - a test mtd device
- * $Id: mtdram.c,v 1.37 2005/04/21 03:42:11 joern Exp $
  * Author: Alexander Larsson <alex@cendio.se>
  *
  * Copyright (c) 1999 Alexander Larsson <alex@cendio.se>
diff --git a/drivers/mtd/devices/phram.c b/drivers/mtd/devices/phram.c
index c7987b1c5e0..088fbb7595b 100644
--- a/drivers/mtd/devices/phram.c
+++ b/drivers/mtd/devices/phram.c
@@ -1,6 +1,4 @@
 /**
- * $Id: phram.c,v 1.16 2005/11/07 11:14:25 gleixner Exp $
- *
  * Copyright (c) ????		Jochen Schäuble <psionic@psionic.de>
  * Copyright (c) 2003-2004	Joern Engel <joern@wh.fh-wedel.de>
  *
diff --git a/drivers/mtd/devices/pmc551.c b/drivers/mtd/devices/pmc551.c
index bc998174906..d38bca64bb1 100644
--- a/drivers/mtd/devices/pmc551.c
+++ b/drivers/mtd/devices/pmc551.c
@@ -1,6 +1,4 @@
 /*
- * $Id: pmc551.c,v 1.32 2005/11/07 11:14:25 gleixner Exp $
- *
  * PMC551 PCI Mezzanine Ram Device
  *
  * Author:
diff --git a/drivers/mtd/devices/slram.c b/drivers/mtd/devices/slram.c
index cb86db746f2..a425d09f35a 100644
--- a/drivers/mtd/devices/slram.c
+++ b/drivers/mtd/devices/slram.c
@@ -1,7 +1,5 @@
 /*======================================================================
 
-  $Id: slram.c,v 1.36 2005/11/07 11:14:25 gleixner Exp $
-
   This driver provides a method to access memory not used by the kernel
   itself (i.e. if the kernel commandline mem=xxx is used). To actually
   use slram at least mtdblock or mtdchar is required (for block or
diff --git a/drivers/mtd/ftl.c b/drivers/mtd/ftl.c
index 5c29872184e..f34f20c7891 100644
--- a/drivers/mtd/ftl.c
+++ b/drivers/mtd/ftl.c
@@ -1,5 +1,4 @@
 /* This version ported to the Linux-MTD system by dwmw2@infradead.org
- * $Id: ftl.c,v 1.59 2005/11/29 14:48:31 gleixner Exp $
  *
  * Fixes: Arnaldo Carvalho de Melo <acme@conectiva.com.br>
  * - fixes some leaks on failure in build_maps and ftl_notify_add, cleanups
@@ -1078,8 +1077,6 @@ static struct mtd_blktrans_ops ftl_tr = {
 
 static int init_ftl(void)
 {
-	DEBUG(0, "$Id: ftl.c,v 1.59 2005/11/29 14:48:31 gleixner Exp $\n");
-
 	return register_mtd_blktrans(&ftl_tr);
 }
 
diff --git a/drivers/mtd/inftlcore.c b/drivers/mtd/inftlcore.c
index b0e396504e6..c4f9d3378b2 100644
--- a/drivers/mtd/inftlcore.c
+++ b/drivers/mtd/inftlcore.c
@@ -7,8 +7,6 @@
  * (c) 1999 Machine Vision Holdings, Inc.
  * Author: David Woodhouse <dwmw2@infradead.org>
  *
- * $Id: inftlcore.c,v 1.19 2005/11/07 11:14:20 gleixner Exp $
- *
  * This program is free software; 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
@@ -953,9 +951,6 @@ static struct mtd_blktrans_ops inftl_tr = {
 
 static int __init init_inftl(void)
 {
-	printk(KERN_INFO "INFTL: inftlcore.c $Revision: 1.19 $, "
-		"inftlmount.c %s\n", inftlmountrev);
-
 	return register_mtd_blktrans(&inftl_tr);
 }
 
diff --git a/drivers/mtd/inftlmount.c b/drivers/mtd/inftlmount.c
index c551d2f0779..9113628ed1e 100644
--- a/drivers/mtd/inftlmount.c
+++ b/drivers/mtd/inftlmount.c
@@ -8,8 +8,6 @@
  * Author: Fabrice Bellard (fabrice.bellard@netgem.com)
  * Copyright (C) 2000 Netgem S.A.
  *
- * $Id: inftlmount.c,v 1.18 2005/11/07 11:14:20 gleixner Exp $
- *
  * This program is free software; 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
@@ -39,8 +37,6 @@
 #include <linux/mtd/inftl.h>
 #include <linux/mtd/compatmac.h>
 
-char inftlmountrev[]="$Revision: 1.18 $";
-
 /*
  * find_boot_record: Find the INFTL Media Header and its Spare copy which
  *	contains the various device information of the INFTL partition and
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index d2fbc296452..df8e00bba07 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -1,5 +1,4 @@
 # drivers/mtd/maps/Kconfig
-# $Id: Kconfig,v 1.61 2005/11/07 11:14:26 gleixner Exp $
 
 menu "Mapping drivers for chip access"
 	depends on MTD!=n
@@ -510,6 +509,17 @@ config MTD_PCMCIA_ANONYMOUS
 
 	  If unsure, say N.
 
+config MTD_BFIN_ASYNC
+	tristate "Blackfin BF533-STAMP Flash Chip Support"
+	depends on BFIN533_STAMP && MTD_CFI
+	select MTD_PARTITIONS
+	default y
+	help
+	  Map driver which allows for simultaneous utilization of
+	  ethernet and CFI parallel flash.
+
+	  If compiled as a module, it will be called bfin-async-flash.
+
 config MTD_UCLINUX
 	tristate "Generic uClinux RAM/ROM filesystem support"
 	depends on MTD_PARTITIONS && !MMU
@@ -539,24 +549,6 @@ config MTD_DMV182
         help
           Map driver for Dy-4 SVME/DMV-182 board.
 
-config MTD_BAST
-	tristate "Map driver for Simtec BAST (EB2410ITX) or Thorcom VR1000"
-	depends on ARCH_BAST || MACH_VR1000
-	select MTD_PARTITIONS
-	select MTD_MAP_BANK_WIDTH_16
-	select MTD_JEDECPROBE
-	help
-	  Map driver for NOR flash on the Simtec BAST (EB2410ITX), or the
-	  Thorcom VR1000
-
-	  Note, this driver *cannot* over-ride the WP link on the
-	  board, or currently detect the state of the link.
-
-config MTD_BAST_MAXSIZE
-	int "Maximum size for BAST flash area (MiB)"
-	depends on MTD_BAST
-	default "4"
-
 config MTD_SHARP_SL
 	tristate "ROM mapped on Sharp SL Series"
 	depends on ARCH_PXA
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index c6ce8673dab..6cda6df973e 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -1,7 +1,6 @@
 #
 # linux/drivers/maps/Makefile
 #
-# $Id: Makefile.common,v 1.34 2005/11/07 11:14:26 gleixner Exp $
 
 ifeq ($(CONFIG_MTD_COMPLEX_MAPPINGS),y)
 obj-$(CONFIG_MTD)		+= map_funcs.o
@@ -10,7 +9,6 @@ endif
 # Chip mappings
 obj-$(CONFIG_MTD_CDB89712)	+= cdb89712.o
 obj-$(CONFIG_MTD_ARM_INTEGRATOR)+= integrator-flash.o
-obj-$(CONFIG_MTD_BAST)		+= bast-flash.o
 obj-$(CONFIG_MTD_CFI_FLAGADM)	+= cfi_flagadm.o
 obj-$(CONFIG_MTD_DC21285)	+= dc21285.o
 obj-$(CONFIG_MTD_DILNETPC)	+= dilnetpc.o
@@ -66,3 +64,4 @@ obj-$(CONFIG_MTD_SHARP_SL)	+= sharpsl-flash.o
 obj-$(CONFIG_MTD_PLATRAM)	+= plat-ram.o
 obj-$(CONFIG_MTD_OMAP_NOR)	+= omap_nor.o
 obj-$(CONFIG_MTD_INTEL_VR_NOR)	+= intel_vr_nor.o
+obj-$(CONFIG_MTD_BFIN_ASYNC)	+= bfin-async-flash.o
diff --git a/drivers/mtd/maps/amd76xrom.c b/drivers/mtd/maps/amd76xrom.c
index 728aed6ad72..948b86f35ef 100644
--- a/drivers/mtd/maps/amd76xrom.c
+++ b/drivers/mtd/maps/amd76xrom.c
@@ -2,7 +2,6 @@
  * amd76xrom.c
  *
  * Normal mappings of chips in physical memory
- * $Id: amd76xrom.c,v 1.21 2005/11/07 11:14:26 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/autcpu12-nvram.c b/drivers/mtd/maps/autcpu12-nvram.c
index 7ed3424dd95..cf32267263d 100644
--- a/drivers/mtd/maps/autcpu12-nvram.c
+++ b/drivers/mtd/maps/autcpu12-nvram.c
@@ -2,8 +2,6 @@
  * NV-RAM memory access on autcpu12
  * (C) 2002 Thomas Gleixner (gleixner@autronix.de)
  *
- * $Id: autcpu12-nvram.c,v 1.9 2005/11/07 11:14:26 gleixner Exp $
- *
  * This program is free software; 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
diff --git a/drivers/mtd/maps/bast-flash.c b/drivers/mtd/maps/bast-flash.c
deleted file mode 100644
index 1f492062f8c..00000000000
--- a/drivers/mtd/maps/bast-flash.c
+++ /dev/null
@@ -1,226 +0,0 @@
-/* linux/drivers/mtd/maps/bast-flash.c
- *
- * Copyright (c) 2004-2005 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * Simtec Bast (EB2410ITX) NOR MTD Mapping driver
- *
- * Changelog:
- *	20-Sep-2004  BJD  Initial version
- *	17-Jan-2005  BJD  Add whole device if no partitions found
- *
- * $Id: bast-flash.c,v 1.5 2005/11/07 11:14:26 gleixner Exp $
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
-*/
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/ioport.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-#include <linux/platform_device.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-#include <asm/io.h>
-#include <asm/mach/flash.h>
-
-#include <asm/arch/map.h>
-#include <asm/arch/bast-map.h>
-#include <asm/arch/bast-cpld.h>
-
-#ifdef CONFIG_MTD_BAST_MAXSIZE
-#define AREA_MAXSIZE (CONFIG_MTD_BAST_MAXSIZE * SZ_1M)
-#else
-#define AREA_MAXSIZE (32 * SZ_1M)
-#endif
-
-#define PFX "bast-flash: "
-
-struct bast_flash_info {
-	struct mtd_info		*mtd;
-	struct map_info		 map;
-	struct mtd_partition	*partitions;
-	struct resource		*area;
-};
-
-static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
-
-static void bast_flash_setrw(int to)
-{
-	unsigned int val;
-	unsigned long flags;
-
-	local_irq_save(flags);
-	val = __raw_readb(BAST_VA_CTRL3);
-
-	if (to)
-		val |= BAST_CPLD_CTRL3_ROMWEN;
-	else
-		val &= ~BAST_CPLD_CTRL3_ROMWEN;
-
-	pr_debug("new cpld ctrl3=%02x\n", val);
-
-	__raw_writeb(val, BAST_VA_CTRL3);
-	local_irq_restore(flags);
-}
-
-static int bast_flash_remove(struct platform_device *pdev)
-{
-	struct bast_flash_info *info = platform_get_drvdata(pdev);
-
-	platform_set_drvdata(pdev, NULL);
-
-	if (info == NULL)
-		return 0;
-
-	if (info->map.virt != NULL)
-		iounmap(info->map.virt);
-
-	if (info->mtd) {
-		del_mtd_partitions(info->mtd);
-		map_destroy(info->mtd);
-	}
-
-	kfree(info->partitions);
-
-	if (info->area) {
-		release_resource(info->area);
-		kfree(info->area);
-	}
-
-	kfree(info);
-
-	return 0;
-}
-
-static int bast_flash_probe(struct platform_device *pdev)
-{
-	struct bast_flash_info *info;
-	struct resource *res;
-	int err = 0;
-
-	info = kmalloc(sizeof(*info), GFP_KERNEL);
-	if (info == NULL) {
-		printk(KERN_ERR PFX "no memory for flash info\n");
-		err = -ENOMEM;
-		goto exit_error;
-	}
-
-	memzero(info, sizeof(*info));
-	platform_set_drvdata(pdev, info);
-
-	res = pdev->resource;  /* assume that the flash has one resource */
-
-	info->map.phys = res->start;
-	info->map.size = res->end - res->start + 1;
-	info->map.name = pdev->dev.bus_id;
-	info->map.bankwidth = 2;
-
-	if (info->map.size > AREA_MAXSIZE)
-		info->map.size = AREA_MAXSIZE;
-
-	pr_debug("%s: area %08lx, size %ld\n", __func__,
-		 info->map.phys, info->map.size);
-
-	info->area = request_mem_region(res->start, info->map.size,
-					pdev->name);
-	if (info->area == NULL) {
-		printk(KERN_ERR PFX "cannot reserve flash memory region\n");
-		err = -ENOENT;
-		goto exit_error;
-	}
-
-	info->map.virt = ioremap(res->start, info->map.size);
-	pr_debug("%s: virt at %08x\n", __func__, (int)info->map.virt);
-
-	if (info->map.virt == 0) {
-		printk(KERN_ERR PFX "failed to ioremap() region\n");
-		err = -EIO;
-		goto exit_error;
-	}
-
-	simple_map_init(&info->map);
-
-	/* enable the write to the flash area */
-
-	bast_flash_setrw(1);
-
-	/* probe for the device(s) */
-
-	info->mtd = do_map_probe("jedec_probe", &info->map);
-	if (info->mtd == NULL)
-		info->mtd = do_map_probe("cfi_probe", &info->map);
-
-	if (info->mtd == NULL) {
-		printk(KERN_ERR PFX "map_probe() failed\n");
-		err = -ENXIO;
-		goto exit_error;
-	}
-
-	/* mark ourselves as the owner */
-	info->mtd->owner = THIS_MODULE;
-
-	err = parse_mtd_partitions(info->mtd, probes, &info->partitions, 0);
-	if (err > 0) {
-		err = add_mtd_partitions(info->mtd, info->partitions, err);
-		if (err)
-			printk(KERN_ERR PFX "cannot add/parse partitions\n");
-	} else {
-		err = add_mtd_device(info->mtd);
-	}
-
-	if (err == 0)
-		return 0;
-
-	/* fall through to exit error */
-
- exit_error:
-	bast_flash_remove(pdev);
-	return err;
-}
-
-static struct platform_driver bast_flash_driver = {
-	.probe		= bast_flash_probe,
-	.remove		= bast_flash_remove,
-	.driver		= {
-		.name	= "bast-nor",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init bast_flash_init(void)
-{
-	printk("BAST NOR-Flash Driver, (c) 2004 Simtec Electronics\n");
-	return platform_driver_register(&bast_flash_driver);
-}
-
-static void __exit bast_flash_exit(void)
-{
-	platform_driver_unregister(&bast_flash_driver);
-}
-
-module_init(bast_flash_init);
-module_exit(bast_flash_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_DESCRIPTION("BAST MTD Map driver");
-MODULE_ALIAS("platform:bast-nor");
diff --git a/drivers/mtd/maps/bfin-async-flash.c b/drivers/mtd/maps/bfin-async-flash.c
new file mode 100644
index 00000000000..6fec86aaed7
--- /dev/null
+++ b/drivers/mtd/maps/bfin-async-flash.c
@@ -0,0 +1,219 @@
+/*
+ * drivers/mtd/maps/bfin-async-flash.c
+ *
+ * Handle the case where flash memory and ethernet mac/phy are
+ * mapped onto the same async bank.  The BF533-STAMP does this
+ * for example.  All board-specific configuration goes in your
+ * board resources file.
+ *
+ * Copyright 2000 Nicolas Pitre <nico@cam.org>
+ * Copyright 2005-2008 Analog Devices Inc.
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/physmap.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+
+#include <asm/blackfin.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <asm/unaligned.h>
+
+#define pr_devinit(fmt, args...) ({ static const __devinitconst char __fmt[] = fmt; printk(__fmt, ## args); })
+
+#define DRIVER_NAME "bfin-async-flash"
+
+struct async_state {
+	struct mtd_info *mtd;
+	struct map_info map;
+	int enet_flash_pin;
+	uint32_t flash_ambctl0, flash_ambctl1;
+	uint32_t save_ambctl0, save_ambctl1;
+	unsigned long irq_flags;
+};
+
+static void switch_to_flash(struct async_state *state)
+{
+	local_irq_save(state->irq_flags);
+
+	gpio_set_value(state->enet_flash_pin, 0);
+
+	state->save_ambctl0 = bfin_read_EBIU_AMBCTL0();
+	state->save_ambctl1 = bfin_read_EBIU_AMBCTL1();
+	bfin_write_EBIU_AMBCTL0(state->flash_ambctl0);
+	bfin_write_EBIU_AMBCTL1(state->flash_ambctl1);
+	SSYNC();
+}
+
+static void switch_back(struct async_state *state)
+{
+	bfin_write_EBIU_AMBCTL0(state->save_ambctl0);
+	bfin_write_EBIU_AMBCTL1(state->save_ambctl1);
+	SSYNC();
+
+	gpio_set_value(state->enet_flash_pin, 1);
+
+	local_irq_restore(state->irq_flags);
+}
+
+static map_word bfin_read(struct map_info *map, unsigned long ofs)
+{
+	struct async_state *state = (struct async_state *)map->map_priv_1;
+	uint16_t word;
+	map_word test;
+
+	switch_to_flash(state);
+
+	word = readw(map->virt + ofs);
+
+	switch_back(state);
+
+	test.x[0] = word;
+	return test;
+}
+
+static void bfin_copy_from(struct map_info *map, void *to, unsigned long from, ssize_t len)
+{
+	struct async_state *state = (struct async_state *)map->map_priv_1;
+
+	switch_to_flash(state);
+
+	memcpy(to, map->virt + from, len);
+
+	switch_back(state);
+}
+
+static void bfin_write(struct map_info *map, map_word d1, unsigned long ofs)
+{
+	struct async_state *state = (struct async_state *)map->map_priv_1;
+	uint16_t d;
+
+	d = d1.x[0];
+
+	switch_to_flash(state);
+
+	writew(d, map->virt + ofs);
+	SSYNC();
+
+	switch_back(state);
+}
+
+static void bfin_copy_to(struct map_info *map, unsigned long to, const void *from, ssize_t len)
+{
+	struct async_state *state = (struct async_state *)map->map_priv_1;
+
+	switch_to_flash(state);
+
+	memcpy(map->virt + to, from, len);
+	SSYNC();
+
+	switch_back(state);
+}
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *part_probe_types[] = { "cmdlinepart", "RedBoot", NULL };
+#endif
+
+static int __devinit bfin_flash_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct physmap_flash_data *pdata = pdev->dev.platform_data;
+	struct resource *memory = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct resource *flash_ambctl = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	struct async_state *state;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->map.name       = DRIVER_NAME;
+	state->map.read       = bfin_read;
+	state->map.copy_from  = bfin_copy_from;
+	state->map.write      = bfin_write;
+	state->map.copy_to    = bfin_copy_to;
+	state->map.bankwidth  = pdata->width;
+	state->map.size       = memory->end - memory->start + 1;
+	state->map.virt       = (void __iomem *)memory->start;
+	state->map.phys       = memory->start;
+	state->map.map_priv_1 = (unsigned long)state;
+	state->enet_flash_pin = platform_get_irq(pdev, 0);
+	state->flash_ambctl0  = flash_ambctl->start;
+	state->flash_ambctl1  = flash_ambctl->end;
+
+	if (gpio_request(state->enet_flash_pin, DRIVER_NAME)) {
+		pr_devinit(KERN_ERR DRIVER_NAME ": Failed to request gpio %d\n", state->enet_flash_pin);
+		return -EBUSY;
+	}
+	gpio_direction_output(state->enet_flash_pin, 1);
+
+	pr_devinit(KERN_NOTICE DRIVER_NAME ": probing %d-bit flash bus\n", state->map.bankwidth * 8);
+	state->mtd = do_map_probe(memory->name, &state->map);
+	if (!state->mtd)
+		return -ENXIO;
+
+#ifdef CONFIG_MTD_PARTITIONS
+	ret = parse_mtd_partitions(state->mtd, part_probe_types, &pdata->parts, 0);
+	if (ret > 0) {
+		pr_devinit(KERN_NOTICE DRIVER_NAME ": Using commandline partition definition\n");
+		add_mtd_partitions(state->mtd, pdata->parts, ret);
+
+	} else if (pdata->nr_parts) {
+		pr_devinit(KERN_NOTICE DRIVER_NAME ": Using board partition definition\n");
+		add_mtd_partitions(state->mtd, pdata->parts, pdata->nr_parts);
+
+	} else
+#endif
+	{
+		pr_devinit(KERN_NOTICE DRIVER_NAME ": no partition info available, registering whole flash at once\n");
+		add_mtd_device(state->mtd);
+	}
+
+	platform_set_drvdata(pdev, state);
+
+	return 0;
+}
+
+static int __devexit bfin_flash_remove(struct platform_device *pdev)
+{
+	struct async_state *state = platform_get_drvdata(pdev);
+	gpio_free(state->enet_flash_pin);
+#ifdef CONFIG_MTD_PARTITIONS
+	del_mtd_partitions(state->mtd);
+#endif
+	map_destroy(state->mtd);
+	kfree(state);
+	return 0;
+}
+
+static struct platform_driver bfin_flash_driver = {
+	.probe		= bfin_flash_probe,
+	.remove		= __devexit_p(bfin_flash_remove),
+	.driver		= {
+		.name	= DRIVER_NAME,
+	},
+};
+
+static int __init bfin_flash_init(void)
+{
+	return platform_driver_register(&bfin_flash_driver);
+}
+module_init(bfin_flash_init);
+
+static void __exit bfin_flash_exit(void)
+{
+	platform_driver_unregister(&bfin_flash_driver);
+}
+module_exit(bfin_flash_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MTD map driver for Blackfins with flash/ethernet on same async bank");
diff --git a/drivers/mtd/maps/cdb89712.c b/drivers/mtd/maps/cdb89712.c
index 9f17bb6c5a9..cb507da0a87 100644
--- a/drivers/mtd/maps/cdb89712.c
+++ b/drivers/mtd/maps/cdb89712.c
@@ -1,7 +1,6 @@
 /*
  * Flash on Cirrus CDB89712
  *
- * $Id: cdb89712.c,v 1.11 2005/11/07 11:14:26 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/ceiva.c b/drivers/mtd/maps/ceiva.c
index 629e6e2641a..6464d487eb1 100644
--- a/drivers/mtd/maps/ceiva.c
+++ b/drivers/mtd/maps/ceiva.c
@@ -11,7 +11,6 @@
  *
  * (C) 2000 Nicolas Pitre <nico@cam.org>
  *
- * $Id: ceiva.c,v 1.11 2004/09/16 23:27:12 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/cfi_flagadm.c b/drivers/mtd/maps/cfi_flagadm.c
index 65e5ee55201..0ecc3f6d735 100644
--- a/drivers/mtd/maps/cfi_flagadm.c
+++ b/drivers/mtd/maps/cfi_flagadm.c
@@ -1,8 +1,6 @@
 /*
  *  Copyright © 2001 Flaga hf. Medical Devices, Kári Davíðsson <kd@flaga.is>
  *
- *  $Id: cfi_flagadm.c,v 1.15 2005/11/07 11:14:26 gleixner Exp $
- *
  *  This program is free software; 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
diff --git a/drivers/mtd/maps/dbox2-flash.c b/drivers/mtd/maps/dbox2-flash.c
index 92a9c7fac99..e115667bf1d 100644
--- a/drivers/mtd/maps/dbox2-flash.c
+++ b/drivers/mtd/maps/dbox2-flash.c
@@ -1,6 +1,4 @@
 /*
- * $Id: dbox2-flash.c,v 1.14 2005/11/07 11:14:26 gleixner Exp $
- *
  * D-Box 2 flash driver
  */
 
diff --git a/drivers/mtd/maps/dc21285.c b/drivers/mtd/maps/dc21285.c
index b32bb9347d7..3aa018c092f 100644
--- a/drivers/mtd/maps/dc21285.c
+++ b/drivers/mtd/maps/dc21285.c
@@ -4,8 +4,6 @@
  * (C) 2000  Nicolas Pitre <nico@cam.org>
  *
  * This code is GPL
- *
- * $Id: dc21285.c,v 1.24 2005/11/07 11:14:26 gleixner Exp $
  */
 #include <linux/module.h>
 #include <linux/types.h>
diff --git a/drivers/mtd/maps/dilnetpc.c b/drivers/mtd/maps/dilnetpc.c
index 1c3b34ad732..0713e3a5a22 100644
--- a/drivers/mtd/maps/dilnetpc.c
+++ b/drivers/mtd/maps/dilnetpc.c
@@ -14,8 +14,6 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
  *
- * $Id: dilnetpc.c,v 1.20 2005/11/07 11:14:26 gleixner Exp $
- *
  * The DIL/Net PC is a tiny embedded PC board made by SSV Embedded Systems
  * featuring the AMD Elan SC410 processor. There are two variants of this
  * board: DNP/1486 and ADNP/1486. The DNP version has 2 megs of flash
diff --git a/drivers/mtd/maps/dmv182.c b/drivers/mtd/maps/dmv182.c
index e0558b0b2fe..d171674eb2e 100644
--- a/drivers/mtd/maps/dmv182.c
+++ b/drivers/mtd/maps/dmv182.c
@@ -4,8 +4,6 @@
  *
  * Flash map driver for the Dy4 SVME182 board
  *
- * $Id: dmv182.c,v 1.6 2005/11/07 11:14:26 gleixner Exp $
- *
  * Copyright 2003-2004, TimeSys Corporation
  *
  * Based on the SVME181 flash map, by Tom Nelson, Dot4, Inc. for TimeSys Corp.
diff --git a/drivers/mtd/maps/ebony.c b/drivers/mtd/maps/ebony.c
index 1488bb92f26..d92b7c70d3e 100644
--- a/drivers/mtd/maps/ebony.c
+++ b/drivers/mtd/maps/ebony.c
@@ -1,6 +1,4 @@
 /*
- * $Id: ebony.c,v 1.16 2005/11/07 11:14:26 gleixner Exp $
- *
  * Mapping for Ebony user flash
  *
  * Matt Porter <mporter@kernel.crashing.org>
diff --git a/drivers/mtd/maps/edb7312.c b/drivers/mtd/maps/edb7312.c
index 1c5b97c8968..9433738c166 100644
--- a/drivers/mtd/maps/edb7312.c
+++ b/drivers/mtd/maps/edb7312.c
@@ -1,6 +1,4 @@
 /*
- * $Id: edb7312.c,v 1.14 2005/11/07 11:14:27 gleixner Exp $
- *
  * Handle mapping of the NOR flash on Cogent EDB7312 boards
  *
  * Copyright 2002 SYSGO Real-Time Solutions GmbH
diff --git a/drivers/mtd/maps/fortunet.c b/drivers/mtd/maps/fortunet.c
index 7c50c271651..a8e3fde4cbd 100644
--- a/drivers/mtd/maps/fortunet.c
+++ b/drivers/mtd/maps/fortunet.c
@@ -1,6 +1,5 @@
 /* fortunet.c memory map
  *
- * $Id: fortunet.c,v 1.11 2005/11/07 11:14:27 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/h720x-flash.c b/drivers/mtd/maps/h720x-flash.c
index 6dde3182d64..ef891547446 100644
--- a/drivers/mtd/maps/h720x-flash.c
+++ b/drivers/mtd/maps/h720x-flash.c
@@ -2,8 +2,6 @@
  * Flash memory access on Hynix GMS30C7201/HMS30C7202 based
  * evaluation boards
  *
- * $Id: h720x-flash.c,v 1.12 2005/11/07 11:14:27 gleixner Exp $
- *
  * (C) 2002 Jungjun Kim <jungjun.kim@hynix.com>
  *     2003 Thomas Gleixner <tglx@linutronix.de>
  */
diff --git a/drivers/mtd/maps/ichxrom.c b/drivers/mtd/maps/ichxrom.c
index 2c884c49e84..aeb6c916e23 100644
--- a/drivers/mtd/maps/ichxrom.c
+++ b/drivers/mtd/maps/ichxrom.c
@@ -2,7 +2,6 @@
  * ichxrom.c
  *
  * Normal mappings of chips in physical memory
- * $Id: ichxrom.c,v 1.19 2005/11/07 11:14:27 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/impa7.c b/drivers/mtd/maps/impa7.c
index a0b4dc7155d..2682ab51a36 100644
--- a/drivers/mtd/maps/impa7.c
+++ b/drivers/mtd/maps/impa7.c
@@ -1,6 +1,4 @@
 /*
- * $Id: impa7.c,v 1.14 2005/11/07 11:14:27 gleixner Exp $
- *
  * Handle mapping of the NOR flash on implementa A7 boards
  *
  * Copyright 2002 SYSGO Real-Time Solutions GmbH
diff --git a/drivers/mtd/maps/integrator-flash.c b/drivers/mtd/maps/integrator-flash.c
index 325c8880c43..ee361aaadb1 100644
--- a/drivers/mtd/maps/integrator-flash.c
+++ b/drivers/mtd/maps/integrator-flash.c
@@ -22,8 +22,6 @@
    This is access code for flashes using ARM's flash partitioning
    standards.
 
-   $Id: integrator-flash.c,v 1.20 2005/11/07 11:14:27 gleixner Exp $
-
 ======================================================================*/
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/ipaq-flash.c b/drivers/mtd/maps/ipaq-flash.c
index f27c132794c..a806119797e 100644
--- a/drivers/mtd/maps/ipaq-flash.c
+++ b/drivers/mtd/maps/ipaq-flash.c
@@ -4,8 +4,6 @@
  * (C) 2000 Nicolas Pitre <nico@cam.org>
  * (C) 2002 Hewlett-Packard Company <jamey.hicks@hp.com>
  * (C) 2003 Christian Pellegrin <chri@ascensit.com>, <chri@infis.univ.ts.it>: concatenation of multiple flashes
- *
- * $Id: ipaq-flash.c,v 1.5 2005/11/07 11:14:27 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/ixp2000.c b/drivers/mtd/maps/ixp2000.c
index c8396b8574c..c2264792a20 100644
--- a/drivers/mtd/maps/ixp2000.c
+++ b/drivers/mtd/maps/ixp2000.c
@@ -1,6 +1,4 @@
 /*
- * $Id: ixp2000.c,v 1.9 2005/11/07 11:14:27 gleixner Exp $
- *
  * drivers/mtd/maps/ixp2000.c
  *
  * Mapping for the Intel XScale IXP2000 based systems
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index 01f19a4714b..9c7a5fbd4e5 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -1,6 +1,4 @@
 /*
- * $Id: ixp4xx.c,v 1.13 2005/11/16 16:23:21 dvrabel Exp $
- *
  * drivers/mtd/maps/ixp4xx.c
  *
  * MTD Map file for IXP4XX based systems. Please do not make per-board
diff --git a/drivers/mtd/maps/l440gx.c b/drivers/mtd/maps/l440gx.c
index 67620adf481..9e054503c4c 100644
--- a/drivers/mtd/maps/l440gx.c
+++ b/drivers/mtd/maps/l440gx.c
@@ -1,6 +1,4 @@
 /*
- * $Id: l440gx.c,v 1.18 2005/11/07 11:14:27 gleixner Exp $
- *
  * BIOS Flash chip on Intel 440GX board.
  *
  * Bugs this currently does not work under linuxBIOS.
diff --git a/drivers/mtd/maps/map_funcs.c b/drivers/mtd/maps/map_funcs.c
index 9105e6ca0aa..3f268370eec 100644
--- a/drivers/mtd/maps/map_funcs.c
+++ b/drivers/mtd/maps/map_funcs.c
@@ -1,6 +1,4 @@
 /*
- * $Id: map_funcs.c,v 1.10 2005/06/06 23:04:36 tpoynor Exp $
- *
  * Out-of-line map I/O functions for simple maps when CONFIG_COMPLEX_MAPPINGS
  * is enabled.
  */
diff --git a/drivers/mtd/maps/mbx860.c b/drivers/mtd/maps/mbx860.c
index 06b11872784..706f67394b0 100644
--- a/drivers/mtd/maps/mbx860.c
+++ b/drivers/mtd/maps/mbx860.c
@@ -1,6 +1,4 @@
 /*
- * $Id: mbx860.c,v 1.9 2005/11/07 11:14:27 gleixner Exp $
- *
  * Handle mapping of the flash on MBX860 boards
  *
  * Author:	Anton Todorov
diff --git a/drivers/mtd/maps/netsc520.c b/drivers/mtd/maps/netsc520.c
index 95dcab2146a..c0cb319b2b7 100644
--- a/drivers/mtd/maps/netsc520.c
+++ b/drivers/mtd/maps/netsc520.c
@@ -3,8 +3,6 @@
  * Copyright (C) 2001 Mark Langsdorf (mark.langsdorf@amd.com)
  *	based on sc520cdp.c by Sysgo Real-Time Solutions GmbH
  *
- * $Id: netsc520.c,v 1.14 2005/11/07 11:14:27 gleixner Exp $
- *
  * This program is free software; 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
diff --git a/drivers/mtd/maps/nettel.c b/drivers/mtd/maps/nettel.c
index 0c9b305a72e..965e6c6d6ab 100644
--- a/drivers/mtd/maps/nettel.c
+++ b/drivers/mtd/maps/nettel.c
@@ -5,8 +5,6 @@
  *
  *      (C) Copyright 2000-2001, Greg Ungerer (gerg@snapgear.com)
  *      (C) Copyright 2001-2002, SnapGear (www.snapgear.com)
- *
- *	$Id: nettel.c,v 1.12 2005/11/29 14:30:00 gleixner Exp $
  */
 
 /****************************************************************************/
diff --git a/drivers/mtd/maps/octagon-5066.c b/drivers/mtd/maps/octagon-5066.c
index a6642db3d32..43e04c1d22a 100644
--- a/drivers/mtd/maps/octagon-5066.c
+++ b/drivers/mtd/maps/octagon-5066.c
@@ -1,4 +1,3 @@
-// $Id: octagon-5066.c,v 1.28 2005/11/07 11:14:27 gleixner Exp $
 /* ######################################################################
 
    Octagon 5066 MTD Driver.
diff --git a/drivers/mtd/maps/omap-toto-flash.c b/drivers/mtd/maps/omap-toto-flash.c
index e6e391efbeb..0a60ebbc217 100644
--- a/drivers/mtd/maps/omap-toto-flash.c
+++ b/drivers/mtd/maps/omap-toto-flash.c
@@ -4,8 +4,6 @@
  * jzhang@ti.com (C) 2003 Texas Instruments.
  *
  *  (C) 2002 MontVista Software, Inc.
- *
- * $Id: omap-toto-flash.c,v 1.5 2005/11/07 11:14:27 gleixner Exp $
  */
 
 #include <linux/module.h>
diff --git a/drivers/mtd/maps/pci.c b/drivers/mtd/maps/pci.c
index d2ab1bae9c3..5c6a25c9038 100644
--- a/drivers/mtd/maps/pci.c
+++ b/drivers/mtd/maps/pci.c
@@ -7,8 +7,6 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- *  $Id: pci.c,v 1.14 2005/11/17 08:20:27 dwmw2 Exp $
- *
  * Generic PCI memory map driver.  We support the following boards:
  *  - Intel IQ80310 ATU.
  *  - Intel EBSA285 (blank rom programming mode). Tested working 27/09/2001
diff --git a/drivers/mtd/maps/pcmciamtd.c b/drivers/mtd/maps/pcmciamtd.c
index 0cc31675aeb..90924fb0048 100644
--- a/drivers/mtd/maps/pcmciamtd.c
+++ b/drivers/mtd/maps/pcmciamtd.c
@@ -1,6 +1,4 @@
 /*
- * $Id: pcmciamtd.c,v 1.55 2005/11/07 11:14:28 gleixner Exp $
- *
  * pcmciamtd.c - MTD driver for PCMCIA flash memory cards
  *
  * Author: Simon Evans <spse@secret.org.uk>
@@ -48,7 +46,6 @@ static const int debug = 0;
 
 
 #define DRIVER_DESC	"PCMCIA Flash memory card driver"
-#define DRIVER_VERSION	"$Revision: 1.55 $"
 
 /* Size of the PCMCIA address space: 26 bits = 64 MB */
 #define MAX_PCMCIA_ADDR	0x4000000
@@ -785,7 +782,7 @@ static struct pcmcia_driver pcmciamtd_driver = {
 
 static int __init init_pcmciamtd(void)
 {
-	info(DRIVER_DESC " " DRIVER_VERSION);
+	info(DRIVER_DESC);
 
 	if(bankwidth && bankwidth != 1 && bankwidth != 2) {
 		info("bad bankwidth (%d), using default", bankwidth);
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index 183255fcfdc..42d844f8f6b 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -1,6 +1,4 @@
 /*
- * $Id: physmap.c,v 1.39 2005/11/29 14:49:36 gleixner Exp $
- *
  * Normal mappings of chips in physical memory
  *
  * Copyright (C) 2003 MontaVista Software Inc.
@@ -203,7 +201,19 @@ static int physmap_flash_suspend(struct platform_device *dev, pm_message_t state
 	int i;
 
 	for (i = 0; i < MAX_RESOURCES && info->mtd[i]; i++)
-		ret |= info->mtd[i]->suspend(info->mtd[i]);
+		if (info->mtd[i]->suspend) {
+			ret = info->mtd[i]->suspend(info->mtd[i]);
+			if (ret)
+				goto fail;
+		}
+
+	return 0;
+fail:
+	for (--i; i >= 0; --i)
+		if (info->mtd[i]->suspend) {
+			BUG_ON(!info->mtd[i]->resume);
+			info->mtd[i]->resume(info->mtd[i]);
+		}
 
 	return ret;
 }
@@ -214,7 +224,8 @@ static int physmap_flash_resume(struct platform_device *dev)
 	int i;
 
 	for (i = 0; i < MAX_RESOURCES && info->mtd[i]; i++)
-		info->mtd[i]->resume(info->mtd[i]);
+		if (info->mtd[i]->resume)
+			info->mtd[i]->resume(info->mtd[i]);
 
 	return 0;
 }
@@ -225,8 +236,9 @@ static void physmap_flash_shutdown(struct platform_device *dev)
 	int i;
 
 	for (i = 0; i < MAX_RESOURCES && info->mtd[i]; i++)
-		if (info->mtd[i]->suspend(info->mtd[i]) == 0)
-			info->mtd[i]->resume(info->mtd[i]);
+		if (info->mtd[i]->suspend && info->mtd[i]->resume)
+			if (info->mtd[i]->suspend(info->mtd[i]) == 0)
+				info->mtd[i]->resume(info->mtd[i]);
 }
 #else
 #define physmap_flash_suspend NULL
diff --git a/drivers/mtd/maps/plat-ram.c b/drivers/mtd/maps/plat-ram.c
index 3eb2643b232..e7dd9c8a965 100644
--- a/drivers/mtd/maps/plat-ram.c
+++ b/drivers/mtd/maps/plat-ram.c
@@ -6,8 +6,6 @@
  *
  * Generic platfrom device based RAM map
  *
- * $Id: plat-ram.c,v 1.7 2005/11/07 11:14:28 gleixner Exp $
- *
  * This program is free software; 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
diff --git a/drivers/mtd/maps/redwood.c b/drivers/mtd/maps/redwood.c
index 4d858b3d5f8..de002eb1a7f 100644
--- a/drivers/mtd/maps/redwood.c
+++ b/drivers/mtd/maps/redwood.c
@@ -1,6 +1,4 @@
 /*
- * $Id: redwood.c,v 1.11 2005/11/07 11:14:28 gleixner Exp $
- *
  * drivers/mtd/maps/redwood.c
  *
  * FLASH map for the IBM Redwood 4/5/6 boards.
diff --git a/drivers/mtd/maps/rpxlite.c b/drivers/mtd/maps/rpxlite.c
index 809a0c8e7aa..14d90edb443 100644
--- a/drivers/mtd/maps/rpxlite.c
+++ b/drivers/mtd/maps/rpxlite.c
@@ -1,6 +1,4 @@
 /*
- * $Id: rpxlite.c,v 1.22 2004/11/04 13:24:15 gleixner Exp $
- *
  * Handle mapping of the flash on the RPX Lite and CLLF boards
  */
 
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index c7d5a52a2d5..e177a43dfff 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -2,8 +2,6 @@
  * Flash memory access on SA11x0 based devices
  *
  * (C) 2000 Nicolas Pitre <nico@cam.org>
- *
- * $Id: sa1100-flash.c,v 1.51 2005/11/07 11:14:28 gleixner Exp $
  */
 #include <linux/module.h>
 #include <linux/types.h>
diff --git a/drivers/mtd/maps/sbc8240.c b/drivers/mtd/maps/sbc8240.c
index b8c1331b7a0..6e1e99cd2b5 100644
--- a/drivers/mtd/maps/sbc8240.c
+++ b/drivers/mtd/maps/sbc8240.c
@@ -4,9 +4,6 @@
  * Carolyn Smith, Tektronix, Inc.
  *
  * This code is GPLed
- *
- * $Id: sbc8240.c,v 1.5 2005/11/07 11:14:28 gleixner Exp $
- *
  */
 
 /*
diff --git a/drivers/mtd/maps/sbc_gxx.c b/drivers/mtd/maps/sbc_gxx.c
index 7cc4041d096..1b1c0b7e11e 100644
--- a/drivers/mtd/maps/sbc_gxx.c
+++ b/drivers/mtd/maps/sbc_gxx.c
@@ -17,8 +17,6 @@
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
 
-   $Id: sbc_gxx.c,v 1.35 2005/11/07 11:14:28 gleixner Exp $
-
 The SBC-MediaGX / SBC-GXx has up to 16 MiB of
 Intel StrataFlash (28F320/28F640) in x8 mode.
 
diff --git a/drivers/mtd/maps/sc520cdp.c b/drivers/mtd/maps/sc520cdp.c
index 4045e372b90..85c1e56309e 100644
--- a/drivers/mtd/maps/sc520cdp.c
+++ b/drivers/mtd/maps/sc520cdp.c
@@ -16,8 +16,6 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
  *
- * $Id: sc520cdp.c,v 1.23 2005/11/17 08:20:27 dwmw2 Exp $
- *
  *
  * The SC520CDP is an evaluation board for the Elan SC520 processor available
  * from AMD. It has two banks of 32-bit Flash ROM, each 8 Megabytes in size,
diff --git a/drivers/mtd/maps/scb2_flash.c b/drivers/mtd/maps/scb2_flash.c
index 0fc5584324e..21169e6d646 100644
--- a/drivers/mtd/maps/scb2_flash.c
+++ b/drivers/mtd/maps/scb2_flash.c
@@ -1,6 +1,5 @@
 /*
  * MTD map driver for BIOS Flash on Intel SCB2 boards
- * $Id: scb2_flash.c,v 1.12 2005/03/18 14:04:35 gleixner Exp $
  * Copyright (C) 2002 Sun Microsystems, Inc.
  * Tim Hockin <thockin@sun.com>
  *
diff --git a/drivers/mtd/maps/scx200_docflash.c b/drivers/mtd/maps/scx200_docflash.c
index 5e2bce22f37..b5391ebb736 100644
--- a/drivers/mtd/maps/scx200_docflash.c
+++ b/drivers/mtd/maps/scx200_docflash.c
@@ -2,8 +2,6 @@
 
    Copyright (c) 2001,2002 Christer Weinigel <wingel@nano-system.com>
 
-   $Id: scx200_docflash.c,v 1.12 2005/11/07 11:14:28 gleixner Exp $
-
    National Semiconductor SCx200 flash mapped with DOCCS
 */
 
diff --git a/drivers/mtd/maps/sharpsl-flash.c b/drivers/mtd/maps/sharpsl-flash.c
index 917dc778f24..026eab02818 100644
--- a/drivers/mtd/maps/sharpsl-flash.c
+++ b/drivers/mtd/maps/sharpsl-flash.c
@@ -4,8 +4,6 @@
  * Copyright (C) 2001 Lineo Japan, Inc.
  * Copyright (C) 2002  SHARP
  *
- * $Id: sharpsl-flash.c,v 1.7 2005/11/07 11:14:28 gleixner Exp $
- *
  * based on rpxlite.c,v 1.15 2001/10/02 15:05:14 dwmw2 Exp
  *          Handle mapping of the flash on the RPX Lite and CLLF boards
  *
diff --git a/drivers/mtd/maps/solutionengine.c b/drivers/mtd/maps/solutionengine.c
index d76ceef453c..0eb41d9c678 100644
--- a/drivers/mtd/maps/solutionengine.c
+++ b/drivers/mtd/maps/solutionengine.c
@@ -1,6 +1,4 @@
 /*
- * $Id: solutionengine.c,v 1.15 2005/11/07 11:14:28 gleixner Exp $
- *
  * Flash and EPROM on Hitachi Solution Engine and similar boards.
  *
  * (C) 2001 Red Hat, Inc.
diff --git a/drivers/mtd/maps/sun_uflash.c b/drivers/mtd/maps/sun_uflash.c
index 001af7f7ddd..0d7c88396c8 100644
--- a/drivers/mtd/maps/sun_uflash.c
+++ b/drivers/mtd/maps/sun_uflash.c
@@ -1,4 +1,4 @@
-/* $Id: sun_uflash.c,v 1.13 2005/11/07 11:14:28 gleixner Exp $
+/*
  *
  * sun_uflash - Driver implementation for user-programmable flash
  * present on many Sun Microsystems SME boardsets.
diff --git a/drivers/mtd/maps/tqm8xxl.c b/drivers/mtd/maps/tqm8xxl.c
index 52173405731..a5d3d8531fa 100644
--- a/drivers/mtd/maps/tqm8xxl.c
+++ b/drivers/mtd/maps/tqm8xxl.c
@@ -2,8 +2,6 @@
  * Handle mapping of the flash memory access routines
  * on TQM8xxL based devices.
  *
- * $Id: tqm8xxl.c,v 1.15 2005/11/07 11:14:28 gleixner Exp $
- *
  * based on rpxlite.c
  *
  * Copyright(C) 2001 Kirk Lee <kirk@hpc.ee.ntu.edu.tw>
diff --git a/drivers/mtd/maps/ts5500_flash.c b/drivers/mtd/maps/ts5500_flash.c
index b47270e850b..e2147bf11c8 100644
--- a/drivers/mtd/maps/ts5500_flash.c
+++ b/drivers/mtd/maps/ts5500_flash.c
@@ -22,8 +22,6 @@
  * - Drive A and B use the resident flash disk (RFD) flash translation layer.
  * - If you have created your own jffs file system and the bios overwrites
  *   it during boot, try disabling Drive A: and B: in the boot order.
- *
- * $Id: ts5500_flash.c,v 1.5 2005/11/07 11:14:28 gleixner Exp $
  */
 
 #include <linux/init.h>
diff --git a/drivers/mtd/maps/tsunami_flash.c b/drivers/mtd/maps/tsunami_flash.c
index 0f915ac3102..77a8bfc0257 100644
--- a/drivers/mtd/maps/tsunami_flash.c
+++ b/drivers/mtd/maps/tsunami_flash.c
@@ -2,7 +2,6 @@
  * tsunami_flash.c
  *
  * flash chip on alpha ds10...
- * $Id: tsunami_flash.c,v 1.10 2005/11/07 11:14:29 gleixner Exp $
  */
 #include <asm/io.h>
 #include <asm/core_tsunami.h>
diff --git a/drivers/mtd/maps/uclinux.c b/drivers/mtd/maps/uclinux.c
index c42f4b83f68..0dc645f8152 100644
--- a/drivers/mtd/maps/uclinux.c
+++ b/drivers/mtd/maps/uclinux.c
@@ -4,8 +4,6 @@
  *	uclinux.c -- generic memory mapped MTD driver for uclinux
  *
  *	(C) Copyright 2002, Greg Ungerer (gerg@snapgear.com)
- *
- * 	$Id: uclinux.c,v 1.12 2005/11/07 11:14:29 gleixner Exp $
  */
 
 /****************************************************************************/
@@ -15,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/fs.h>
+#include <linux/mm.h>
 #include <linux/major.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
diff --git a/drivers/mtd/maps/vmax301.c b/drivers/mtd/maps/vmax301.c
index b3e48739543..5a0c9a353b0 100644
--- a/drivers/mtd/maps/vmax301.c
+++ b/drivers/mtd/maps/vmax301.c
@@ -1,4 +1,3 @@
-// $Id: vmax301.c,v 1.32 2005/11/07 11:14:29 gleixner Exp $
 /* ######################################################################
 
    Tempustech VMAX SBC301 MTD Driver.
diff --git a/drivers/mtd/maps/walnut.c b/drivers/mtd/maps/walnut.c
index ca932122fb6..e243476c817 100644
--- a/drivers/mtd/maps/walnut.c
+++ b/drivers/mtd/maps/walnut.c
@@ -1,6 +1,4 @@
 /*
- * $Id: walnut.c,v 1.3 2005/11/07 11:14:29 gleixner Exp $
- *
  * Mapping for Walnut flash
  * (used ebony.c as a "framework")
  *
diff --git a/drivers/mtd/maps/wr_sbc82xx_flash.c b/drivers/mtd/maps/wr_sbc82xx_flash.c
index ac5b8105b6e..413b0cf9bbd 100644
--- a/drivers/mtd/maps/wr_sbc82xx_flash.c
+++ b/drivers/mtd/maps/wr_sbc82xx_flash.c
@@ -1,6 +1,4 @@
 /*
- * $Id: wr_sbc82xx_flash.c,v 1.8 2005/11/07 11:14:29 gleixner Exp $
- *
  * Map for flash chips on Wind River PowerQUICC II SBC82xx board.
  *
  * Copyright (C) 2004 Red Hat, Inc.
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 839eed8430a..9ff007c4962 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -1,6 +1,4 @@
 /*
- * $Id: mtd_blkdevs.c,v 1.27 2005/11/07 11:14:20 gleixner Exp $
- *
  * (C) 2003 David Woodhouse <dwmw2@infradead.org>
  *
  * Interface to Linux 2.5 block layer for MTD 'translation layers'.
@@ -212,7 +210,7 @@ static struct block_device_operations mtd_blktrans_ops = {
 int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 {
 	struct mtd_blktrans_ops *tr = new->tr;
-	struct list_head *this;
+	struct mtd_blktrans_dev *d;
 	int last_devnum = -1;
 	struct gendisk *gd;
 
@@ -221,8 +219,7 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 		BUG();
 	}
 
-	list_for_each(this, &tr->devs) {
-		struct mtd_blktrans_dev *d = list_entry(this, struct mtd_blktrans_dev, list);
+	list_for_each_entry(d, &tr->devs, list) {
 		if (new->devnum == -1) {
 			/* Use first free number */
 			if (d->devnum != last_devnum+1) {
@@ -309,33 +306,24 @@ int del_mtd_blktrans_dev(struct mtd_blktrans_dev *old)
 
 static void blktrans_notify_remove(struct mtd_info *mtd)
 {
-	struct list_head *this, *this2, *next;
-
-	list_for_each(this, &blktrans_majors) {
-		struct mtd_blktrans_ops *tr = list_entry(this, struct mtd_blktrans_ops, list);
-
-		list_for_each_safe(this2, next, &tr->devs) {
-			struct mtd_blktrans_dev *dev = list_entry(this2, struct mtd_blktrans_dev, list);
+	struct mtd_blktrans_ops *tr;
+	struct mtd_blktrans_dev *dev, *next;
 
+	list_for_each_entry(tr, &blktrans_majors, list)
+		list_for_each_entry_safe(dev, next, &tr->devs, list)
 			if (dev->mtd == mtd)
 				tr->remove_dev(dev);
-		}
-	}
 }
 
 static void blktrans_notify_add(struct mtd_info *mtd)
 {
-	struct list_head *this;
+	struct mtd_blktrans_ops *tr;
 
 	if (mtd->type == MTD_ABSENT)
 		return;
 
-	list_for_each(this, &blktrans_majors) {
-		struct mtd_blktrans_ops *tr = list_entry(this, struct mtd_blktrans_ops, list);
-
+	list_for_each_entry(tr, &blktrans_majors, list)
 		tr->add_mtd(tr, mtd);
-	}
-
 }
 
 static struct mtd_notifier blktrans_notifier = {
@@ -406,7 +394,7 @@ int register_mtd_blktrans(struct mtd_blktrans_ops *tr)
 
 int deregister_mtd_blktrans(struct mtd_blktrans_ops *tr)
 {
-	struct list_head *this, *next;
+	struct mtd_blktrans_dev *dev, *next;
 
 	mutex_lock(&mtd_table_mutex);
 
@@ -416,10 +404,8 @@ int deregister_mtd_blktrans(struct mtd_blktrans_ops *tr)
 	/* Remove it from the list of active majors */
 	list_del(&tr->list);
 
-	list_for_each_safe(this, next, &tr->devs) {
-		struct mtd_blktrans_dev *dev = list_entry(this, struct mtd_blktrans_dev, list);
+	list_for_each_entry_safe(dev, next, &tr->devs, list)
 		tr->remove_dev(dev);
-	}
 
 	blk_cleanup_queue(tr->blkcore_priv->rq);
 	unregister_blkdev(tr->major, tr->name);
diff --git a/drivers/mtd/mtdblock.c b/drivers/mtd/mtdblock.c
index 952da30b174..208c6faa035 100644
--- a/drivers/mtd/mtdblock.c
+++ b/drivers/mtd/mtdblock.c
@@ -1,8 +1,6 @@
 /*
  * Direct MTD block device access
  *
- * $Id: mtdblock.c,v 1.68 2005/11/07 11:14:20 gleixner Exp $
- *
  * (C) 2000-2003 Nicolas Pitre <nico@cam.org>
  * (C) 1999-2003 David Woodhouse <dwmw2@infradead.org>
  */
diff --git a/drivers/mtd/mtdblock_ro.c b/drivers/mtd/mtdblock_ro.c
index f79dbb49b1a..852165f8b1c 100644
--- a/drivers/mtd/mtdblock_ro.c
+++ b/drivers/mtd/mtdblock_ro.c
@@ -1,6 +1,4 @@
 /*
- * $Id: mtdblock_ro.c,v 1.19 2004/11/16 18:28:59 dwmw2 Exp $
- *
  * (C) 2003 David Woodhouse <dwmw2@infradead.org>
  *
  * Simple read-only (writable only for RAM) mtdblock driver
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index aef9f4b687c..d2f331876e4 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -1,6 +1,4 @@
 /*
- * $Id: mtdchar.c,v 1.76 2005/11/07 11:14:20 gleixner Exp $
- *
  * Character-device access to raw MTD devices.
  *
  */
@@ -494,6 +492,7 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
 	{
 		struct mtd_oob_buf buf;
 		struct mtd_oob_ops ops;
+		struct mtd_oob_buf __user *user_buf = argp;
 	        uint32_t retlen;
 
 		if(!(file->f_mode & 2))
@@ -537,8 +536,7 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
 		if (ops.oobretlen > 0xFFFFFFFFU)
 			ret = -EOVERFLOW;
 		retlen = ops.oobretlen;
-		if (copy_to_user(&((struct mtd_oob_buf *)argp)->length,
-				 &retlen, sizeof(buf.length)))
+		if (copy_to_user(&user_buf->length, &retlen, sizeof(buf.length)))
 			ret = -EFAULT;
 
 		kfree(ops.oobbuf);
@@ -592,29 +590,29 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
 
 	case MEMLOCK:
 	{
-		struct erase_info_user info;
+		struct erase_info_user einfo;
 
-		if (copy_from_user(&info, argp, sizeof(info)))
+		if (copy_from_user(&einfo, argp, sizeof(einfo)))
 			return -EFAULT;
 
 		if (!mtd->lock)
 			ret = -EOPNOTSUPP;
 		else
-			ret = mtd->lock(mtd, info.start, info.length);
+			ret = mtd->lock(mtd, einfo.start, einfo.length);
 		break;
 	}
 
 	case MEMUNLOCK:
 	{
-		struct erase_info_user info;
+		struct erase_info_user einfo;
 
-		if (copy_from_user(&info, argp, sizeof(info)))
+		if (copy_from_user(&einfo, argp, sizeof(einfo)))
 			return -EFAULT;
 
 		if (!mtd->unlock)
 			ret = -EOPNOTSUPP;
 		else
-			ret = mtd->unlock(mtd, info.start, info.length);
+			ret = mtd->unlock(mtd, einfo.start, einfo.length);
 		break;
 	}
 
@@ -714,15 +712,15 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
 
 	case OTPLOCK:
 	{
-		struct otp_info info;
+		struct otp_info oinfo;
 
 		if (mfi->mode != MTD_MODE_OTP_USER)
 			return -EINVAL;
-		if (copy_from_user(&info, argp, sizeof(info)))
+		if (copy_from_user(&oinfo, argp, sizeof(oinfo)))
 			return -EFAULT;
 		if (!mtd->lock_user_prot_reg)
 			return -EOPNOTSUPP;
-		ret = mtd->lock_user_prot_reg(mtd, info.start, info.length);
+		ret = mtd->lock_user_prot_reg(mtd, oinfo.start, oinfo.length);
 		break;
 	}
 #endif
diff --git a/drivers/mtd/mtdconcat.c b/drivers/mtd/mtdconcat.c
index d563dcd4b26..2972a5edb73 100644
--- a/drivers/mtd/mtdconcat.c
+++ b/drivers/mtd/mtdconcat.c
@@ -6,8 +6,6 @@
  * NAND support by Christian Gan <cgan@iders.ca>
  *
  * This code is GPL
- *
- * $Id: mtdconcat.c,v 1.11 2005/11/07 11:14:20 gleixner Exp $
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index f7e7890e5bc..a9d24694982 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -1,6 +1,4 @@
 /*
- * $Id: mtdcore.c,v 1.47 2005/11/07 11:14:20 gleixner Exp $
- *
  * Core registration and callback routines for MTD
  * drivers and users.
  *
@@ -53,7 +51,7 @@ int add_mtd_device(struct mtd_info *mtd)
 
 	for (i=0; i < MAX_MTD_DEVICES; i++)
 		if (!mtd_table[i]) {
-			struct list_head *this;
+			struct mtd_notifier *not;
 
 			mtd_table[i] = mtd;
 			mtd->index = i;
@@ -72,10 +70,8 @@ int add_mtd_device(struct mtd_info *mtd)
 			DEBUG(0, "mtd: Giving out device %d to %s\n",i, mtd->name);
 			/* No need to get a refcount on the module containing
 			   the notifier, since we hold the mtd_table_mutex */
-			list_for_each(this, &mtd_notifiers) {
-				struct mtd_notifier *not = list_entry(this, struct mtd_notifier, list);
+			list_for_each_entry(not, &mtd_notifiers, list)
 				not->add(mtd);
-			}
 
 			mutex_unlock(&mtd_table_mutex);
 			/* We _know_ we aren't being removed, because
@@ -113,14 +109,12 @@ int del_mtd_device (struct mtd_info *mtd)
 		       mtd->index, mtd->name, mtd->usecount);
 		ret = -EBUSY;
 	} else {
-		struct list_head *this;
+		struct mtd_notifier *not;
 
 		/* No need to get a refcount on the module containing
 		   the notifier, since we hold the mtd_table_mutex */
-		list_for_each(this, &mtd_notifiers) {
-			struct mtd_notifier *not = list_entry(this, struct mtd_notifier, list);
+		list_for_each_entry(not, &mtd_notifiers, list)
 			not->remove(mtd);
-		}
 
 		mtd_table[mtd->index] = NULL;
 
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index 07c70116934..edb90b58a9b 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -5,8 +5,6 @@
  *
  * This code is GPL
  *
- * $Id: mtdpart.c,v 1.55 2005/11/07 11:14:20 gleixner Exp $
- *
  * 	02-21-2002	Thomas Gleixner <gleixner@autronix.de>
  *			added support for read_oob, write_oob
  */
@@ -46,8 +44,8 @@ struct mtd_part {
  * to the _real_ device.
  */
 
-static int part_read (struct mtd_info *mtd, loff_t from, size_t len,
-			size_t *retlen, u_char *buf)
+static int part_read(struct mtd_info *mtd, loff_t from, size_t len,
+		size_t *retlen, u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
 	int res;
@@ -56,7 +54,7 @@ static int part_read (struct mtd_info *mtd, loff_t from, size_t len,
 		len = 0;
 	else if (from + len > mtd->size)
 		len = mtd->size - from;
-	res = part->master->read (part->master, from + part->offset,
+	res = part->master->read(part->master, from + part->offset,
 				   len, retlen, buf);
 	if (unlikely(res)) {
 		if (res == -EUCLEAN)
@@ -67,8 +65,8 @@ static int part_read (struct mtd_info *mtd, loff_t from, size_t len,
 	return res;
 }
 
-static int part_point (struct mtd_info *mtd, loff_t from, size_t len,
-			size_t *retlen, void **virt, resource_size_t *phys)
+static int part_point(struct mtd_info *mtd, loff_t from, size_t len,
+		size_t *retlen, void **virt, resource_size_t *phys)
 {
 	struct mtd_part *part = PART(mtd);
 	if (from >= mtd->size)
@@ -87,7 +85,7 @@ static void part_unpoint(struct mtd_info *mtd, loff_t from, size_t len)
 }
 
 static int part_read_oob(struct mtd_info *mtd, loff_t from,
-			 struct mtd_oob_ops *ops)
+		struct mtd_oob_ops *ops)
 {
 	struct mtd_part *part = PART(mtd);
 	int res;
@@ -107,38 +105,38 @@ static int part_read_oob(struct mtd_info *mtd, loff_t from,
 	return res;
 }
 
-static int part_read_user_prot_reg (struct mtd_info *mtd, loff_t from, size_t len,
-			size_t *retlen, u_char *buf)
+static int part_read_user_prot_reg(struct mtd_info *mtd, loff_t from,
+		size_t len, size_t *retlen, u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->read_user_prot_reg (part->master, from,
+	return part->master->read_user_prot_reg(part->master, from,
 					len, retlen, buf);
 }
 
-static int part_get_user_prot_info (struct mtd_info *mtd,
-				    struct otp_info *buf, size_t len)
+static int part_get_user_prot_info(struct mtd_info *mtd,
+		struct otp_info *buf, size_t len)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->get_user_prot_info (part->master, buf, len);
+	return part->master->get_user_prot_info(part->master, buf, len);
 }
 
-static int part_read_fact_prot_reg (struct mtd_info *mtd, loff_t from, size_t len,
-			size_t *retlen, u_char *buf)
+static int part_read_fact_prot_reg(struct mtd_info *mtd, loff_t from,
+		size_t len, size_t *retlen, u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->read_fact_prot_reg (part->master, from,
+	return part->master->read_fact_prot_reg(part->master, from,
 					len, retlen, buf);
 }
 
-static int part_get_fact_prot_info (struct mtd_info *mtd,
-				    struct otp_info *buf, size_t len)
+static int part_get_fact_prot_info(struct mtd_info *mtd, struct otp_info *buf,
+		size_t len)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->get_fact_prot_info (part->master, buf, len);
+	return part->master->get_fact_prot_info(part->master, buf, len);
 }
 
-static int part_write (struct mtd_info *mtd, loff_t to, size_t len,
-			size_t *retlen, const u_char *buf)
+static int part_write(struct mtd_info *mtd, loff_t to, size_t len,
+		size_t *retlen, const u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
 	if (!(mtd->flags & MTD_WRITEABLE))
@@ -147,12 +145,12 @@ static int part_write (struct mtd_info *mtd, loff_t to, size_t len,
 		len = 0;
 	else if (to + len > mtd->size)
 		len = mtd->size - to;
-	return part->master->write (part->master, to + part->offset,
+	return part->master->write(part->master, to + part->offset,
 				    len, retlen, buf);
 }
 
-static int part_panic_write (struct mtd_info *mtd, loff_t to, size_t len,
-			size_t *retlen, const u_char *buf)
+static int part_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
+		size_t *retlen, const u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
 	if (!(mtd->flags & MTD_WRITEABLE))
@@ -161,12 +159,12 @@ static int part_panic_write (struct mtd_info *mtd, loff_t to, size_t len,
 		len = 0;
 	else if (to + len > mtd->size)
 		len = mtd->size - to;
-	return part->master->panic_write (part->master, to + part->offset,
+	return part->master->panic_write(part->master, to + part->offset,
 				    len, retlen, buf);
 }
 
 static int part_write_oob(struct mtd_info *mtd, loff_t to,
-			 struct mtd_oob_ops *ops)
+		struct mtd_oob_ops *ops)
 {
 	struct mtd_part *part = PART(mtd);
 
@@ -180,31 +178,32 @@ static int part_write_oob(struct mtd_info *mtd, loff_t to,
 	return part->master->write_oob(part->master, to + part->offset, ops);
 }
 
-static int part_write_user_prot_reg (struct mtd_info *mtd, loff_t from, size_t len,
-			size_t *retlen, u_char *buf)
+static int part_write_user_prot_reg(struct mtd_info *mtd, loff_t from,
+		size_t len, size_t *retlen, u_char *buf)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->write_user_prot_reg (part->master, from,
+	return part->master->write_user_prot_reg(part->master, from,
 					len, retlen, buf);
 }
 
-static int part_lock_user_prot_reg (struct mtd_info *mtd, loff_t from, size_t len)
+static int part_lock_user_prot_reg(struct mtd_info *mtd, loff_t from,
+		size_t len)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->lock_user_prot_reg (part->master, from, len);
+	return part->master->lock_user_prot_reg(part->master, from, len);
 }
 
-static int part_writev (struct mtd_info *mtd,  const struct kvec *vecs,
-			 unsigned long count, loff_t to, size_t *retlen)
+static int part_writev(struct mtd_info *mtd, const struct kvec *vecs,
+		unsigned long count, loff_t to, size_t *retlen)
 {
 	struct mtd_part *part = PART(mtd);
 	if (!(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
-	return part->master->writev (part->master, vecs, count,
+	return part->master->writev(part->master, vecs, count,
 					to + part->offset, retlen);
 }
 
-static int part_erase (struct mtd_info *mtd, struct erase_info *instr)
+static int part_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
 	struct mtd_part *part = PART(mtd);
 	int ret;
@@ -236,7 +235,7 @@ void mtd_erase_callback(struct erase_info *instr)
 }
 EXPORT_SYMBOL_GPL(mtd_erase_callback);
 
-static int part_lock (struct mtd_info *mtd, loff_t ofs, size_t len)
+static int part_lock(struct mtd_info *mtd, loff_t ofs, size_t len)
 {
 	struct mtd_part *part = PART(mtd);
 	if ((len + ofs) > mtd->size)
@@ -244,7 +243,7 @@ static int part_lock (struct mtd_info *mtd, loff_t ofs, size_t len)
 	return part->master->lock(part->master, ofs + part->offset, len);
 }
 
-static int part_unlock (struct mtd_info *mtd, loff_t ofs, size_t len)
+static int part_unlock(struct mtd_info *mtd, loff_t ofs, size_t len)
 {
 	struct mtd_part *part = PART(mtd);
 	if ((len + ofs) > mtd->size)
@@ -270,7 +269,7 @@ static void part_resume(struct mtd_info *mtd)
 	part->master->resume(part->master);
 }
 
-static int part_block_isbad (struct mtd_info *mtd, loff_t ofs)
+static int part_block_isbad(struct mtd_info *mtd, loff_t ofs)
 {
 	struct mtd_part *part = PART(mtd);
 	if (ofs >= mtd->size)
@@ -279,7 +278,7 @@ static int part_block_isbad (struct mtd_info *mtd, loff_t ofs)
 	return part->master->block_isbad(part->master, ofs);
 }
 
-static int part_block_markbad (struct mtd_info *mtd, loff_t ofs)
+static int part_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
 	struct mtd_part *part = PART(mtd);
 	int res;
@@ -302,229 +301,237 @@ static int part_block_markbad (struct mtd_info *mtd, loff_t ofs)
 
 int del_mtd_partitions(struct mtd_info *master)
 {
-	struct list_head *node;
-	struct mtd_part *slave;
+	struct mtd_part *slave, *next;
 
-	for (node = mtd_partitions.next;
-	     node != &mtd_partitions;
-	     node = node->next) {
-		slave = list_entry(node, struct mtd_part, list);
+	list_for_each_entry_safe(slave, next, &mtd_partitions, list)
 		if (slave->master == master) {
-			struct list_head *prev = node->prev;
-			__list_del(prev, node->next);
-			if(slave->registered)
+			list_del(&slave->list);
+			if (slave->registered)
 				del_mtd_device(&slave->mtd);
 			kfree(slave);
-			node = prev;
 		}
-	}
 
 	return 0;
 }
+EXPORT_SYMBOL(del_mtd_partitions);
 
-/*
- * This function, given a master MTD object and a partition table, creates
- * and registers slave MTD objects which are bound to the master according to
- * the partition definitions.
- * (Q: should we register the master MTD object as well?)
- */
-
-int add_mtd_partitions(struct mtd_info *master,
-		       const struct mtd_partition *parts,
-		       int nbparts)
+static struct mtd_part *add_one_partition(struct mtd_info *master,
+		const struct mtd_partition *part, int partno,
+		u_int32_t cur_offset)
 {
 	struct mtd_part *slave;
-	u_int32_t cur_offset = 0;
-	int i;
-
-	printk (KERN_NOTICE "Creating %d MTD partitions on \"%s\":\n", nbparts, master->name);
-
-	for (i = 0; i < nbparts; i++) {
 
-		/* allocate the partition structure */
-		slave = kzalloc (sizeof(*slave), GFP_KERNEL);
-		if (!slave) {
-			printk ("memory allocation error while creating partitions for \"%s\"\n",
-				master->name);
-			del_mtd_partitions(master);
-			return -ENOMEM;
-		}
-		list_add(&slave->list, &mtd_partitions);
+	/* allocate the partition structure */
+	slave = kzalloc(sizeof(*slave), GFP_KERNEL);
+	if (!slave) {
+		printk(KERN_ERR"memory allocation error while creating partitions for \"%s\"\n",
+			master->name);
+		del_mtd_partitions(master);
+		return NULL;
+	}
+	list_add(&slave->list, &mtd_partitions);
 
-		/* set up the MTD object for this partition */
-		slave->mtd.type = master->type;
-		slave->mtd.flags = master->flags & ~parts[i].mask_flags;
-		slave->mtd.size = parts[i].size;
-		slave->mtd.writesize = master->writesize;
-		slave->mtd.oobsize = master->oobsize;
-		slave->mtd.oobavail = master->oobavail;
-		slave->mtd.subpage_sft = master->subpage_sft;
+	/* set up the MTD object for this partition */
+	slave->mtd.type = master->type;
+	slave->mtd.flags = master->flags & ~part->mask_flags;
+	slave->mtd.size = part->size;
+	slave->mtd.writesize = master->writesize;
+	slave->mtd.oobsize = master->oobsize;
+	slave->mtd.oobavail = master->oobavail;
+	slave->mtd.subpage_sft = master->subpage_sft;
 
-		slave->mtd.name = parts[i].name;
-		slave->mtd.owner = master->owner;
+	slave->mtd.name = part->name;
+	slave->mtd.owner = master->owner;
 
-		slave->mtd.read = part_read;
-		slave->mtd.write = part_write;
+	slave->mtd.read = part_read;
+	slave->mtd.write = part_write;
 
-		if (master->panic_write)
-			slave->mtd.panic_write = part_panic_write;
+	if (master->panic_write)
+		slave->mtd.panic_write = part_panic_write;
 
-		if(master->point && master->unpoint){
-			slave->mtd.point = part_point;
-			slave->mtd.unpoint = part_unpoint;
-		}
+	if (master->point && master->unpoint) {
+		slave->mtd.point = part_point;
+		slave->mtd.unpoint = part_unpoint;
+	}
 
-		if (master->read_oob)
-			slave->mtd.read_oob = part_read_oob;
-		if (master->write_oob)
-			slave->mtd.write_oob = part_write_oob;
-		if(master->read_user_prot_reg)
-			slave->mtd.read_user_prot_reg = part_read_user_prot_reg;
-		if(master->read_fact_prot_reg)
-			slave->mtd.read_fact_prot_reg = part_read_fact_prot_reg;
-		if(master->write_user_prot_reg)
-			slave->mtd.write_user_prot_reg = part_write_user_prot_reg;
-		if(master->lock_user_prot_reg)
-			slave->mtd.lock_user_prot_reg = part_lock_user_prot_reg;
-		if(master->get_user_prot_info)
-			slave->mtd.get_user_prot_info = part_get_user_prot_info;
-		if(master->get_fact_prot_info)
-			slave->mtd.get_fact_prot_info = part_get_fact_prot_info;
-		if (master->sync)
-			slave->mtd.sync = part_sync;
-		if (!i && master->suspend && master->resume) {
-				slave->mtd.suspend = part_suspend;
-				slave->mtd.resume = part_resume;
+	if (master->read_oob)
+		slave->mtd.read_oob = part_read_oob;
+	if (master->write_oob)
+		slave->mtd.write_oob = part_write_oob;
+	if (master->read_user_prot_reg)
+		slave->mtd.read_user_prot_reg = part_read_user_prot_reg;
+	if (master->read_fact_prot_reg)
+		slave->mtd.read_fact_prot_reg = part_read_fact_prot_reg;
+	if (master->write_user_prot_reg)
+		slave->mtd.write_user_prot_reg = part_write_user_prot_reg;
+	if (master->lock_user_prot_reg)
+		slave->mtd.lock_user_prot_reg = part_lock_user_prot_reg;
+	if (master->get_user_prot_info)
+		slave->mtd.get_user_prot_info = part_get_user_prot_info;
+	if (master->get_fact_prot_info)
+		slave->mtd.get_fact_prot_info = part_get_fact_prot_info;
+	if (master->sync)
+		slave->mtd.sync = part_sync;
+	if (!partno && master->suspend && master->resume) {
+			slave->mtd.suspend = part_suspend;
+			slave->mtd.resume = part_resume;
+	}
+	if (master->writev)
+		slave->mtd.writev = part_writev;
+	if (master->lock)
+		slave->mtd.lock = part_lock;
+	if (master->unlock)
+		slave->mtd.unlock = part_unlock;
+	if (master->block_isbad)
+		slave->mtd.block_isbad = part_block_isbad;
+	if (master->block_markbad)
+		slave->mtd.block_markbad = part_block_markbad;
+	slave->mtd.erase = part_erase;
+	slave->master = master;
+	slave->offset = part->offset;
+	slave->index = partno;
+
+	if (slave->offset == MTDPART_OFS_APPEND)
+		slave->offset = cur_offset;
+	if (slave->offset == MTDPART_OFS_NXTBLK) {
+		slave->offset = cur_offset;
+		if ((cur_offset % master->erasesize) != 0) {
+			/* Round up to next erasesize */
+			slave->offset = ((cur_offset / master->erasesize) + 1) * master->erasesize;
+			printk(KERN_NOTICE "Moving partition %d: "
+			       "0x%08x -> 0x%08x\n", partno,
+			       cur_offset, slave->offset);
 		}
-		if (master->writev)
-			slave->mtd.writev = part_writev;
-		if (master->lock)
-			slave->mtd.lock = part_lock;
-		if (master->unlock)
-			slave->mtd.unlock = part_unlock;
-		if (master->block_isbad)
-			slave->mtd.block_isbad = part_block_isbad;
-		if (master->block_markbad)
-			slave->mtd.block_markbad = part_block_markbad;
-		slave->mtd.erase = part_erase;
-		slave->master = master;
-		slave->offset = parts[i].offset;
-		slave->index = i;
-
-		if (slave->offset == MTDPART_OFS_APPEND)
-			slave->offset = cur_offset;
-		if (slave->offset == MTDPART_OFS_NXTBLK) {
-			slave->offset = cur_offset;
-			if ((cur_offset % master->erasesize) != 0) {
-				/* Round up to next erasesize */
-				slave->offset = ((cur_offset / master->erasesize) + 1) * master->erasesize;
-				printk(KERN_NOTICE "Moving partition %d: "
-				       "0x%08x -> 0x%08x\n", i,
-				       cur_offset, slave->offset);
+	}
+	if (slave->mtd.size == MTDPART_SIZ_FULL)
+		slave->mtd.size = master->size - slave->offset;
+
+	printk(KERN_NOTICE "0x%08x-0x%08x : \"%s\"\n", slave->offset,
+		slave->offset + slave->mtd.size, slave->mtd.name);
+
+	/* let's do some sanity checks */
+	if (slave->offset >= master->size) {
+		/* let's register it anyway to preserve ordering */
+		slave->offset = 0;
+		slave->mtd.size = 0;
+		printk(KERN_ERR"mtd: partition \"%s\" is out of reach -- disabled\n",
+			part->name);
+		goto out_register;
+	}
+	if (slave->offset + slave->mtd.size > master->size) {
+		slave->mtd.size = master->size - slave->offset;
+		printk(KERN_WARNING"mtd: partition \"%s\" extends beyond the end of device \"%s\" -- size truncated to %#x\n",
+			part->name, master->name, slave->mtd.size);
+	}
+	if (master->numeraseregions > 1) {
+		/* Deal with variable erase size stuff */
+		int i, max = master->numeraseregions;
+		u32 end = slave->offset + slave->mtd.size;
+		struct mtd_erase_region_info *regions = master->eraseregions;
+
+		/* Find the first erase regions which is part of this
+		 * partition. */
+		for (i = 0; i < max && regions[i].offset <= slave->offset; i++)
+			;
+		/* The loop searched for the region _behind_ the first one */
+		i--;
+
+		/* Pick biggest erasesize */
+		for (; i < max && regions[i].offset < end; i++) {
+			if (slave->mtd.erasesize < regions[i].erasesize) {
+				slave->mtd.erasesize = regions[i].erasesize;
 			}
 		}
-		if (slave->mtd.size == MTDPART_SIZ_FULL)
-			slave->mtd.size = master->size - slave->offset;
-		cur_offset = slave->offset + slave->mtd.size;
+		BUG_ON(slave->mtd.erasesize == 0);
+	} else {
+		/* Single erase size */
+		slave->mtd.erasesize = master->erasesize;
+	}
 
-		printk (KERN_NOTICE "0x%08x-0x%08x : \"%s\"\n", slave->offset,
-			slave->offset + slave->mtd.size, slave->mtd.name);
+	if ((slave->mtd.flags & MTD_WRITEABLE) &&
+	    (slave->offset % slave->mtd.erasesize)) {
+		/* Doesn't start on a boundary of major erase size */
+		/* FIXME: Let it be writable if it is on a boundary of
+		 * _minor_ erase size though */
+		slave->mtd.flags &= ~MTD_WRITEABLE;
+		printk(KERN_WARNING"mtd: partition \"%s\" doesn't start on an erase block boundary -- force read-only\n",
+			part->name);
+	}
+	if ((slave->mtd.flags & MTD_WRITEABLE) &&
+	    (slave->mtd.size % slave->mtd.erasesize)) {
+		slave->mtd.flags &= ~MTD_WRITEABLE;
+		printk(KERN_WARNING"mtd: partition \"%s\" doesn't end on an erase block -- force read-only\n",
+			part->name);
+	}
 
-		/* let's do some sanity checks */
-		if (slave->offset >= master->size) {
-				/* let's register it anyway to preserve ordering */
-			slave->offset = 0;
-			slave->mtd.size = 0;
-			printk ("mtd: partition \"%s\" is out of reach -- disabled\n",
-				parts[i].name);
-		}
-		if (slave->offset + slave->mtd.size > master->size) {
-			slave->mtd.size = master->size - slave->offset;
-			printk ("mtd: partition \"%s\" extends beyond the end of device \"%s\" -- size truncated to %#x\n",
-				parts[i].name, master->name, slave->mtd.size);
-		}
-		if (master->numeraseregions>1) {
-			/* Deal with variable erase size stuff */
-			int i;
-			struct mtd_erase_region_info *regions = master->eraseregions;
-
-			/* Find the first erase regions which is part of this partition. */
-			for (i=0; i < master->numeraseregions && slave->offset >= regions[i].offset; i++)
-				;
-
-			for (i--; i < master->numeraseregions && slave->offset + slave->mtd.size > regions[i].offset; i++) {
-				if (slave->mtd.erasesize < regions[i].erasesize) {
-					slave->mtd.erasesize = regions[i].erasesize;
-				}
-			}
-		} else {
-			/* Single erase size */
-			slave->mtd.erasesize = master->erasesize;
-		}
+	slave->mtd.ecclayout = master->ecclayout;
+	if (master->block_isbad) {
+		uint32_t offs = 0;
 
-		if ((slave->mtd.flags & MTD_WRITEABLE) &&
-		    (slave->offset % slave->mtd.erasesize)) {
-			/* Doesn't start on a boundary of major erase size */
-			/* FIXME: Let it be writable if it is on a boundary of _minor_ erase size though */
-			slave->mtd.flags &= ~MTD_WRITEABLE;
-			printk ("mtd: partition \"%s\" doesn't start on an erase block boundary -- force read-only\n",
-				parts[i].name);
-		}
-		if ((slave->mtd.flags & MTD_WRITEABLE) &&
-		    (slave->mtd.size % slave->mtd.erasesize)) {
-			slave->mtd.flags &= ~MTD_WRITEABLE;
-			printk ("mtd: partition \"%s\" doesn't end on an erase block -- force read-only\n",
-				parts[i].name);
+		while (offs < slave->mtd.size) {
+			if (master->block_isbad(master,
+						offs + slave->offset))
+				slave->mtd.ecc_stats.badblocks++;
+			offs += slave->mtd.erasesize;
 		}
+	}
 
-		slave->mtd.ecclayout = master->ecclayout;
-		if (master->block_isbad) {
-			uint32_t offs = 0;
+out_register:
+	if (part->mtdp) {
+		/* store the object pointer (caller may or may not register it*/
+		*part->mtdp = &slave->mtd;
+		slave->registered = 0;
+	} else {
+		/* register our partition */
+		add_mtd_device(&slave->mtd);
+		slave->registered = 1;
+	}
+	return slave;
+}
 
-			while(offs < slave->mtd.size) {
-				if (master->block_isbad(master,
-							offs + slave->offset))
-					slave->mtd.ecc_stats.badblocks++;
-				offs += slave->mtd.erasesize;
-			}
-		}
+/*
+ * This function, given a master MTD object and a partition table, creates
+ * and registers slave MTD objects which are bound to the master according to
+ * the partition definitions.
+ * (Q: should we register the master MTD object as well?)
+ */
 
-		if(parts[i].mtdp)
-		{	/* store the object pointer (caller may or may not register it */
-			*parts[i].mtdp = &slave->mtd;
-			slave->registered = 0;
-		}
-		else
-		{
-			/* register our partition */
-			add_mtd_device(&slave->mtd);
-			slave->registered = 1;
-		}
+int add_mtd_partitions(struct mtd_info *master,
+		       const struct mtd_partition *parts,
+		       int nbparts)
+{
+	struct mtd_part *slave;
+	u_int32_t cur_offset = 0;
+	int i;
+
+	printk(KERN_NOTICE "Creating %d MTD partitions on \"%s\":\n", nbparts, master->name);
+
+	for (i = 0; i < nbparts; i++) {
+		slave = add_one_partition(master, parts + i, i, cur_offset);
+		if (!slave)
+			return -ENOMEM;
+		cur_offset = slave->offset + slave->mtd.size;
 	}
 
 	return 0;
 }
-
 EXPORT_SYMBOL(add_mtd_partitions);
-EXPORT_SYMBOL(del_mtd_partitions);
 
 static DEFINE_SPINLOCK(part_parser_lock);
 static LIST_HEAD(part_parsers);
 
 static struct mtd_part_parser *get_partition_parser(const char *name)
 {
-	struct list_head *this;
-	void *ret = NULL;
-	spin_lock(&part_parser_lock);
+	struct mtd_part_parser *p, *ret = NULL;
 
-	list_for_each(this, &part_parsers) {
-		struct mtd_part_parser *p = list_entry(this, struct mtd_part_parser, list);
+	spin_lock(&part_parser_lock);
 
+	list_for_each_entry(p, &part_parsers, list)
 		if (!strcmp(p->name, name) && try_module_get(p->owner)) {
 			ret = p;
 			break;
 		}
-	}
+
 	spin_unlock(&part_parser_lock);
 
 	return ret;
@@ -538,6 +545,7 @@ int register_mtd_parser(struct mtd_part_parser *p)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(register_mtd_parser);
 
 int deregister_mtd_parser(struct mtd_part_parser *p)
 {
@@ -546,6 +554,7 @@ int deregister_mtd_parser(struct mtd_part_parser *p)
 	spin_unlock(&part_parser_lock);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(deregister_mtd_parser);
 
 int parse_mtd_partitions(struct mtd_info *master, const char **types,
 			 struct mtd_partition **pparts, unsigned long origin)
@@ -573,7 +582,4 @@ int parse_mtd_partitions(struct mtd_info *master, const char **types,
 	}
 	return ret;
 }
-
 EXPORT_SYMBOL_GPL(parse_mtd_partitions);
-EXPORT_SYMBOL_GPL(register_mtd_parser);
-EXPORT_SYMBOL_GPL(deregister_mtd_parser);
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 5076faf9ca6..71406e51785 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -1,5 +1,4 @@
 # drivers/mtd/nand/Kconfig
-# $Id: Kconfig,v 1.35 2005/11/07 11:14:30 gleixner Exp $
 
 menuconfig MTD_NAND
 	tristate "NAND Device Support"
@@ -272,22 +271,23 @@ config MTD_NAND_CS553X
 
 	  If you say "m", the module will be called "cs553x_nand.ko".
 
-config MTD_NAND_AT91
-	bool "Support for NAND Flash / SmartMedia on AT91"
-	depends on ARCH_AT91
+config MTD_NAND_ATMEL
+	tristate "Support for NAND Flash / SmartMedia on AT91 and AVR32"
+	depends on ARCH_AT91 || AVR32
 	help
 	  Enables support for NAND Flash / Smart Media Card interface
-	  on Atmel AT91 processors.
+	  on Atmel AT91 and AVR32 processors.
 choice
-	prompt "ECC management for NAND Flash / SmartMedia on AT91"
-	depends on MTD_NAND_AT91
+	prompt "ECC management for NAND Flash / SmartMedia on AT91 / AVR32"
+	depends on MTD_NAND_ATMEL
 
-config MTD_NAND_AT91_ECC_HW
+config MTD_NAND_ATMEL_ECC_HW
 	bool "Hardware ECC"
-	depends on ARCH_AT91SAM9263 || ARCH_AT91SAM9260
+	depends on ARCH_AT91SAM9263 || ARCH_AT91SAM9260 || AVR32
 	help
-	  Uses hardware ECC provided by the at91sam9260/at91sam9263 chip
-	  instead of software ECC.
+	  Use hardware ECC instead of software ECC when the chip
+	  supports it.
+
 	  The hardware ECC controller is capable of single bit error
 	  correction and 2-bit random detection per page.
 
@@ -297,16 +297,16 @@ config MTD_NAND_AT91_ECC_HW
 
 	  If unsure, say Y
 
-config MTD_NAND_AT91_ECC_SOFT
+config MTD_NAND_ATMEL_ECC_SOFT
 	bool "Software ECC"
 	help
-	  Uses software ECC.
+	  Use software ECC.
 
 	  NB : hardware and software ECC schemes are incompatible.
 	  If you switch from one to another, you'll have to erase your
 	  mtd partition.
 
-config MTD_NAND_AT91_ECC_NONE
+config MTD_NAND_ATMEL_ECC_NONE
 	bool "No ECC (testing only, DANGEROUS)"
 	depends on DEBUG_KERNEL
 	help
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index a6e74a46992..d772581de57 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -1,7 +1,6 @@
 #
 # linux/drivers/nand/Makefile
 #
-# $Id: Makefile.common,v 1.15 2004/11/26 12:28:22 dedekind Exp $
 
 obj-$(CONFIG_MTD_NAND)			+= nand.o nand_ecc.o
 obj-$(CONFIG_MTD_NAND_IDS)		+= nand_ids.o
@@ -24,7 +23,7 @@ obj-$(CONFIG_MTD_NAND_TS7250)		+= ts7250.o
 obj-$(CONFIG_MTD_NAND_NANDSIM)		+= nandsim.o
 obj-$(CONFIG_MTD_NAND_CS553X)		+= cs553x_nand.o
 obj-$(CONFIG_MTD_NAND_NDFC)		+= ndfc.o
-obj-$(CONFIG_MTD_NAND_AT91)		+= at91_nand.o
+obj-$(CONFIG_MTD_NAND_ATMEL)		+= atmel_nand.o
 obj-$(CONFIG_MTD_NAND_CM_X270)		+= cmx270_nand.o
 obj-$(CONFIG_MTD_NAND_BASLER_EXCITE)	+= excite_nandflash.o
 obj-$(CONFIG_MTD_NAND_PXA3xx)		+= pxa3xx_nand.o
diff --git a/drivers/mtd/nand/at91_nand.c b/drivers/mtd/nand/atmel_nand.c
index 0adb287027a..99aec46e214 100644
--- a/drivers/mtd/nand/at91_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -1,6 +1,4 @@
 /*
- * drivers/mtd/nand/at91_nand.c
- *
  *  Copyright (C) 2003 Rick Bronson
  *
  *  Derived from drivers/mtd/nand/autcpu12.c
@@ -31,20 +29,19 @@
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
 
-#include <asm/io.h>
-#include <asm/sizes.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
 
-#include <asm/hardware.h>
 #include <asm/arch/board.h>
-#include <asm/arch/gpio.h>
+#include <asm/arch/cpu.h>
 
-#ifdef CONFIG_MTD_NAND_AT91_ECC_HW
+#ifdef CONFIG_MTD_NAND_ATMEL_ECC_HW
 #define hard_ecc	1
 #else
 #define hard_ecc	0
 #endif
 
-#ifdef CONFIG_MTD_NAND_AT91_ECC_NONE
+#ifdef CONFIG_MTD_NAND_ATMEL_ECC_NONE
 #define no_ecc		1
 #else
 #define no_ecc		0
@@ -52,18 +49,18 @@
 
 /* Register access macros */
 #define ecc_readl(add, reg)				\
-	__raw_readl(add + AT91_ECC_##reg)
+	__raw_readl(add + ATMEL_ECC_##reg)
 #define ecc_writel(add, reg, value)			\
-	__raw_writel((value), add + AT91_ECC_##reg)
+	__raw_writel((value), add + ATMEL_ECC_##reg)
 
-#include <asm/arch/at91_ecc.h> /* AT91SAM9260/3 ECC registers */
+#include "atmel_nand_ecc.h"	/* Hardware ECC registers */
 
 /* oob layout for large page size
  * bad block info is on bytes 0 and 1
  * the bytes have to be consecutives to avoid
  * several NAND_CMD_RNDOUT during read
  */
-static struct nand_ecclayout at91_oobinfo_large = {
+static struct nand_ecclayout atmel_oobinfo_large = {
 	.eccbytes = 4,
 	.eccpos = {60, 61, 62, 63},
 	.oobfree = {
@@ -76,7 +73,7 @@ static struct nand_ecclayout at91_oobinfo_large = {
  * the bytes have to be consecutives to avoid
  * several NAND_CMD_RNDOUT during read
  */
-static struct nand_ecclayout at91_oobinfo_small = {
+static struct nand_ecclayout atmel_oobinfo_small = {
 	.eccbytes = 4,
 	.eccpos = {0, 1, 2, 3},
 	.oobfree = {
@@ -84,11 +81,11 @@ static struct nand_ecclayout at91_oobinfo_small = {
 	},
 };
 
-struct at91_nand_host {
+struct atmel_nand_host {
 	struct nand_chip	nand_chip;
 	struct mtd_info		mtd;
 	void __iomem		*io_base;
-	struct at91_nand_data	*board;
+	struct atmel_nand_data	*board;
 	struct device		*dev;
 	void __iomem		*ecc;
 };
@@ -96,34 +93,34 @@ struct at91_nand_host {
 /*
  * Enable NAND.
  */
-static void at91_nand_enable(struct at91_nand_host *host)
+static void atmel_nand_enable(struct atmel_nand_host *host)
 {
 	if (host->board->enable_pin)
-		at91_set_gpio_value(host->board->enable_pin, 0);
+		gpio_set_value(host->board->enable_pin, 0);
 }
 
 /*
  * Disable NAND.
  */
-static void at91_nand_disable(struct at91_nand_host *host)
+static void atmel_nand_disable(struct atmel_nand_host *host)
 {
 	if (host->board->enable_pin)
-		at91_set_gpio_value(host->board->enable_pin, 1);
+		gpio_set_value(host->board->enable_pin, 1);
 }
 
 /*
  * Hardware specific access to control-lines
  */
-static void at91_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
+static void atmel_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
 	struct nand_chip *nand_chip = mtd->priv;
-	struct at91_nand_host *host = nand_chip->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		if (ctrl & NAND_NCE)
-			at91_nand_enable(host);
+			atmel_nand_enable(host);
 		else
-			at91_nand_disable(host);
+			atmel_nand_disable(host);
 	}
 	if (cmd == NAND_CMD_NONE)
 		return;
@@ -137,18 +134,49 @@ static void at91_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 /*
  * Read the Device Ready pin.
  */
-static int at91_nand_device_ready(struct mtd_info *mtd)
+static int atmel_nand_device_ready(struct mtd_info *mtd)
 {
 	struct nand_chip *nand_chip = mtd->priv;
-	struct at91_nand_host *host = nand_chip->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
 
-	return at91_get_gpio_value(host->board->rdy_pin);
+	return gpio_get_value(host->board->rdy_pin);
+}
+
+/*
+ * Minimal-overhead PIO for data access.
+ */
+static void atmel_read_buf(struct mtd_info *mtd, u8 *buf, int len)
+{
+	struct nand_chip	*nand_chip = mtd->priv;
+
+	__raw_readsb(nand_chip->IO_ADDR_R, buf, len);
+}
+
+static void atmel_read_buf16(struct mtd_info *mtd, u8 *buf, int len)
+{
+	struct nand_chip	*nand_chip = mtd->priv;
+
+	__raw_readsw(nand_chip->IO_ADDR_R, buf, len / 2);
+}
+
+static void atmel_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
+{
+	struct nand_chip	*nand_chip = mtd->priv;
+
+	__raw_writesb(nand_chip->IO_ADDR_W, buf, len);
+}
+
+static void atmel_write_buf16(struct mtd_info *mtd, const u8 *buf, int len)
+{
+	struct nand_chip	*nand_chip = mtd->priv;
+
+	__raw_writesw(nand_chip->IO_ADDR_W, buf, len / 2);
 }
 
 /*
  * write oob for small pages
  */
-static int at91_nand_write_oob_512(struct mtd_info *mtd,
+static int atmel_nand_write_oob_512(struct mtd_info *mtd,
 		struct nand_chip *chip, int page)
 {
 	int chunk = chip->ecc.bytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -176,7 +204,7 @@ static int at91_nand_write_oob_512(struct mtd_info *mtd,
 /*
  * read oob for small pages
  */
-static int at91_nand_read_oob_512(struct mtd_info *mtd,
+static int atmel_nand_read_oob_512(struct mtd_info *mtd,
 		struct nand_chip *chip,	int page, int sndcmd)
 {
 	if (sndcmd) {
@@ -196,11 +224,11 @@ static int at91_nand_read_oob_512(struct mtd_info *mtd,
  * dat:        raw data (unused)
  * ecc_code:   buffer for ECC
  */
-static int at91_nand_calculate(struct mtd_info *mtd,
+static int atmel_nand_calculate(struct mtd_info *mtd,
 		const u_char *dat, unsigned char *ecc_code)
 {
 	struct nand_chip *nand_chip = mtd->priv;
-	struct at91_nand_host *host = nand_chip->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
 	uint32_t *eccpos = nand_chip->ecc.layout->eccpos;
 	unsigned int ecc_value;
 
@@ -211,7 +239,7 @@ static int at91_nand_calculate(struct mtd_info *mtd,
 	ecc_code[eccpos[1]] = (ecc_value >> 8) & 0xFF;
 
 	/* get the last 2 ECC bytes */
-	ecc_value = ecc_readl(host->ecc, NPR) & AT91_ECC_NPARITY;
+	ecc_value = ecc_readl(host->ecc, NPR) & ATMEL_ECC_NPARITY;
 
 	ecc_code[eccpos[2]] = ecc_value & 0xFF;
 	ecc_code[eccpos[3]] = (ecc_value >> 8) & 0xFF;
@@ -226,7 +254,7 @@ static int at91_nand_calculate(struct mtd_info *mtd,
  * chip:       nand chip info structure
  * buf:        buffer to store read data
  */
-static int at91_nand_read_page(struct mtd_info *mtd,
+static int atmel_nand_read_page(struct mtd_info *mtd,
 		struct nand_chip *chip, uint8_t *buf)
 {
 	int eccsize = chip->ecc.size;
@@ -237,6 +265,19 @@ static int at91_nand_read_page(struct mtd_info *mtd,
 	uint8_t *ecc_pos;
 	int stat;
 
+	/*
+	 * Errata: ALE is incorrectly wired up to the ECC controller
+	 * on the AP7000, so it will include the address cycles in the
+	 * ECC calculation.
+	 *
+	 * Workaround: Reset the parity registers before reading the
+	 * actual data.
+	 */
+	if (cpu_is_at32ap7000()) {
+		struct atmel_nand_host *host = chip->priv;
+		ecc_writel(host->ecc, CR, ATMEL_ECC_RST);
+	}
+
 	/* read the page */
 	chip->read_buf(mtd, p, eccsize);
 
@@ -285,11 +326,11 @@ static int at91_nand_read_page(struct mtd_info *mtd,
  *
  * Detect and correct a 1 bit error for a page
  */
-static int at91_nand_correct(struct mtd_info *mtd, u_char *dat,
+static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
 		u_char *read_ecc, u_char *isnull)
 {
 	struct nand_chip *nand_chip = mtd->priv;
-	struct at91_nand_host *host = nand_chip->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
 	unsigned int ecc_status;
 	unsigned int ecc_word, ecc_bit;
 
@@ -297,43 +338,43 @@ static int at91_nand_correct(struct mtd_info *mtd, u_char *dat,
 	ecc_status = ecc_readl(host->ecc, SR);
 
 	/* if there's no error */
-	if (likely(!(ecc_status & AT91_ECC_RECERR)))
+	if (likely(!(ecc_status & ATMEL_ECC_RECERR)))
 		return 0;
 
 	/* get error bit offset (4 bits) */
-	ecc_bit = ecc_readl(host->ecc, PR) & AT91_ECC_BITADDR;
+	ecc_bit = ecc_readl(host->ecc, PR) & ATMEL_ECC_BITADDR;
 	/* get word address (12 bits) */
-	ecc_word = ecc_readl(host->ecc, PR) & AT91_ECC_WORDADDR;
+	ecc_word = ecc_readl(host->ecc, PR) & ATMEL_ECC_WORDADDR;
 	ecc_word >>= 4;
 
 	/* if there are multiple errors */
-	if (ecc_status & AT91_ECC_MULERR) {
+	if (ecc_status & ATMEL_ECC_MULERR) {
 		/* check if it is a freshly erased block
 		 * (filled with 0xff) */
-		if ((ecc_bit == AT91_ECC_BITADDR)
-				&& (ecc_word == (AT91_ECC_WORDADDR >> 4))) {
+		if ((ecc_bit == ATMEL_ECC_BITADDR)
+				&& (ecc_word == (ATMEL_ECC_WORDADDR >> 4))) {
 			/* the block has just been erased, return OK */
 			return 0;
 		}
 		/* it doesn't seems to be a freshly
 		 * erased block.
 		 * We can't correct so many errors */
-		dev_dbg(host->dev, "at91_nand : multiple errors detected."
+		dev_dbg(host->dev, "atmel_nand : multiple errors detected."
 				" Unable to correct.\n");
 		return -EIO;
 	}
 
 	/* if there's a single bit error : we can correct it */
-	if (ecc_status & AT91_ECC_ECCERR) {
+	if (ecc_status & ATMEL_ECC_ECCERR) {
 		/* there's nothing much to do here.
 		 * the bit error is on the ECC itself.
 		 */
-		dev_dbg(host->dev, "at91_nand : one bit error on ECC code."
+		dev_dbg(host->dev, "atmel_nand : one bit error on ECC code."
 				" Nothing to correct\n");
 		return 0;
 	}
 
-	dev_dbg(host->dev, "at91_nand : one bit error on data."
+	dev_dbg(host->dev, "atmel_nand : one bit error on data."
 			" (word offset in the page :"
 			" 0x%x bit offset : 0x%x)\n",
 			ecc_word, ecc_bit);
@@ -345,14 +386,21 @@ static int at91_nand_correct(struct mtd_info *mtd, u_char *dat,
 		/* 8 bits words */
 		dat[ecc_word] ^= (1 << ecc_bit);
 	}
-	dev_dbg(host->dev, "at91_nand : error corrected\n");
+	dev_dbg(host->dev, "atmel_nand : error corrected\n");
 	return 1;
 }
 
 /*
- * Enable HW ECC : unsused
+ * Enable HW ECC : unused on most chips
  */
-static void at91_nand_hwctl(struct mtd_info *mtd, int mode) { ; }
+static void atmel_nand_hwctl(struct mtd_info *mtd, int mode)
+{
+	if (cpu_is_at32ap7000()) {
+		struct nand_chip *nand_chip = mtd->priv;
+		struct atmel_nand_host *host = nand_chip->priv;
+		ecc_writel(host->ecc, CR, ATMEL_ECC_RST);
+	}
+}
 
 #ifdef CONFIG_MTD_PARTITIONS
 static const char *part_probes[] = { "cmdlinepart", NULL };
@@ -361,9 +409,9 @@ static const char *part_probes[] = { "cmdlinepart", NULL };
 /*
  * Probe for the NAND device.
  */
-static int __init at91_nand_probe(struct platform_device *pdev)
+static int __init atmel_nand_probe(struct platform_device *pdev)
 {
-	struct at91_nand_host *host;
+	struct atmel_nand_host *host;
 	struct mtd_info *mtd;
 	struct nand_chip *nand_chip;
 	struct resource *regs;
@@ -375,24 +423,24 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 	int num_partitions = 0;
 #endif
 
-	/* Allocate memory for the device structure (and zero it) */
-	host = kzalloc(sizeof(struct at91_nand_host), GFP_KERNEL);
-	if (!host) {
-		printk(KERN_ERR "at91_nand: failed to allocate device structure.\n");
-		return -ENOMEM;
-	}
-
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!mem) {
-		printk(KERN_ERR "at91_nand: can't get I/O resource mem\n");
+		printk(KERN_ERR "atmel_nand: can't get I/O resource mem\n");
 		return -ENXIO;
 	}
 
+	/* Allocate memory for the device structure (and zero it) */
+	host = kzalloc(sizeof(struct atmel_nand_host), GFP_KERNEL);
+	if (!host) {
+		printk(KERN_ERR "atmel_nand: failed to allocate device structure.\n");
+		return -ENOMEM;
+	}
+
 	host->io_base = ioremap(mem->start, mem->end - mem->start + 1);
 	if (host->io_base == NULL) {
-		printk(KERN_ERR "at91_nand: ioremap failed\n");
-		kfree(host);
-		return -EIO;
+		printk(KERN_ERR "atmel_nand: ioremap failed\n");
+		res = -EIO;
+		goto err_nand_ioremap;
 	}
 
 	mtd = &host->mtd;
@@ -407,14 +455,14 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 	/* Set address of NAND IO lines */
 	nand_chip->IO_ADDR_R = host->io_base;
 	nand_chip->IO_ADDR_W = host->io_base;
-	nand_chip->cmd_ctrl = at91_nand_cmd_ctrl;
+	nand_chip->cmd_ctrl = atmel_nand_cmd_ctrl;
 
 	if (host->board->rdy_pin)
-		nand_chip->dev_ready = at91_nand_device_ready;
+		nand_chip->dev_ready = atmel_nand_device_ready;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 	if (!regs && hard_ecc) {
-		printk(KERN_ERR "at91_nand: can't get I/O resource "
+		printk(KERN_ERR "atmel_nand: can't get I/O resource "
 				"regs\nFalling back on software ECC\n");
 	}
 
@@ -424,15 +472,15 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 	if (hard_ecc && regs) {
 		host->ecc = ioremap(regs->start, regs->end - regs->start + 1);
 		if (host->ecc == NULL) {
-			printk(KERN_ERR "at91_nand: ioremap failed\n");
+			printk(KERN_ERR "atmel_nand: ioremap failed\n");
 			res = -EIO;
 			goto err_ecc_ioremap;
 		}
 		nand_chip->ecc.mode = NAND_ECC_HW_SYNDROME;
-		nand_chip->ecc.calculate = at91_nand_calculate;
-		nand_chip->ecc.correct = at91_nand_correct;
-		nand_chip->ecc.hwctl = at91_nand_hwctl;
-		nand_chip->ecc.read_page = at91_nand_read_page;
+		nand_chip->ecc.calculate = atmel_nand_calculate;
+		nand_chip->ecc.correct = atmel_nand_correct;
+		nand_chip->ecc.hwctl = atmel_nand_hwctl;
+		nand_chip->ecc.read_page = atmel_nand_read_page;
 		nand_chip->ecc.bytes = 4;
 		nand_chip->ecc.prepad = 0;
 		nand_chip->ecc.postpad = 0;
@@ -440,24 +488,30 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 
 	nand_chip->chip_delay = 20;		/* 20us command delay time */
 
-	if (host->board->bus_width_16)		/* 16-bit bus width */
+	if (host->board->bus_width_16) {	/* 16-bit bus width */
 		nand_chip->options |= NAND_BUSWIDTH_16;
+		nand_chip->read_buf = atmel_read_buf16;
+		nand_chip->write_buf = atmel_write_buf16;
+	} else {
+		nand_chip->read_buf = atmel_read_buf;
+		nand_chip->write_buf = atmel_write_buf;
+	}
 
 	platform_set_drvdata(pdev, host);
-	at91_nand_enable(host);
+	atmel_nand_enable(host);
 
 	if (host->board->det_pin) {
-		if (at91_get_gpio_value(host->board->det_pin)) {
-			printk ("No SmartMedia card inserted.\n");
+		if (gpio_get_value(host->board->det_pin)) {
+			printk("No SmartMedia card inserted.\n");
 			res = ENXIO;
-			goto out;
+			goto err_no_card;
 		}
 	}
 
 	/* first scan to find the device and get the page size */
 	if (nand_scan_ident(mtd, 1)) {
 		res = -ENXIO;
-		goto out;
+		goto err_scan_ident;
 	}
 
 	if (nand_chip->ecc.mode == NAND_ECC_HW_SYNDROME) {
@@ -467,22 +521,22 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 		/* set ECC page size and oob layout */
 		switch (mtd->writesize) {
 		case 512:
-			nand_chip->ecc.layout = &at91_oobinfo_small;
-			nand_chip->ecc.read_oob = at91_nand_read_oob_512;
-			nand_chip->ecc.write_oob = at91_nand_write_oob_512;
-			ecc_writel(host->ecc, MR, AT91_ECC_PAGESIZE_528);
+			nand_chip->ecc.layout = &atmel_oobinfo_small;
+			nand_chip->ecc.read_oob = atmel_nand_read_oob_512;
+			nand_chip->ecc.write_oob = atmel_nand_write_oob_512;
+			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_528);
 			break;
 		case 1024:
-			nand_chip->ecc.layout = &at91_oobinfo_large;
-			ecc_writel(host->ecc, MR, AT91_ECC_PAGESIZE_1056);
+			nand_chip->ecc.layout = &atmel_oobinfo_large;
+			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_1056);
 			break;
 		case 2048:
-			nand_chip->ecc.layout = &at91_oobinfo_large;
-			ecc_writel(host->ecc, MR, AT91_ECC_PAGESIZE_2112);
+			nand_chip->ecc.layout = &atmel_oobinfo_large;
+			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_2112);
 			break;
 		case 4096:
-			nand_chip->ecc.layout = &at91_oobinfo_large;
-			ecc_writel(host->ecc, MR, AT91_ECC_PAGESIZE_4224);
+			nand_chip->ecc.layout = &atmel_oobinfo_large;
+			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_4224);
 			break;
 		default:
 			/* page size not handled by HW ECC */
@@ -502,12 +556,12 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 	/* second phase scan */
 	if (nand_scan_tail(mtd)) {
 		res = -ENXIO;
-		goto out;
+		goto err_scan_tail;
 	}
 
 #ifdef CONFIG_MTD_PARTITIONS
 #ifdef CONFIG_MTD_CMDLINE_PARTS
-	mtd->name = "at91_nand";
+	mtd->name = "atmel_nand";
 	num_partitions = parse_mtd_partitions(mtd, part_probes,
 					      &partitions, 0);
 #endif
@@ -516,9 +570,9 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 							 &num_partitions);
 
 	if ((!partitions) || (num_partitions == 0)) {
-		printk(KERN_ERR "at91_nand: No parititions defined, or unsupported device.\n");
+		printk(KERN_ERR "atmel_nand: No parititions defined, or unsupported device.\n");
 		res = ENXIO;
-		goto release;
+		goto err_no_partitions;
 	}
 
 	res = add_mtd_partitions(mtd, partitions, num_partitions);
@@ -530,17 +584,19 @@ static int __init at91_nand_probe(struct platform_device *pdev)
 		return res;
 
 #ifdef CONFIG_MTD_PARTITIONS
-release:
+err_no_partitions:
 #endif
 	nand_release(mtd);
-
-out:
-	iounmap(host->ecc);
-
-err_ecc_ioremap:
-	at91_nand_disable(host);
+err_scan_tail:
+err_scan_ident:
+err_no_card:
+	atmel_nand_disable(host);
 	platform_set_drvdata(pdev, NULL);
+	if (host->ecc)
+		iounmap(host->ecc);
+err_ecc_ioremap:
 	iounmap(host->io_base);
+err_nand_ioremap:
 	kfree(host);
 	return res;
 }
@@ -548,47 +604,47 @@ err_ecc_ioremap:
 /*
  * Remove a NAND device.
  */
-static int __devexit at91_nand_remove(struct platform_device *pdev)
+static int __exit atmel_nand_remove(struct platform_device *pdev)
 {
-	struct at91_nand_host *host = platform_get_drvdata(pdev);
+	struct atmel_nand_host *host = platform_get_drvdata(pdev);
 	struct mtd_info *mtd = &host->mtd;
 
 	nand_release(mtd);
 
-	at91_nand_disable(host);
+	atmel_nand_disable(host);
 
+	if (host->ecc)
+		iounmap(host->ecc);
 	iounmap(host->io_base);
-	iounmap(host->ecc);
 	kfree(host);
 
 	return 0;
 }
 
-static struct platform_driver at91_nand_driver = {
-	.probe		= at91_nand_probe,
-	.remove		= at91_nand_remove,
+static struct platform_driver atmel_nand_driver = {
+	.remove		= __exit_p(atmel_nand_remove),
 	.driver		= {
-		.name	= "at91_nand",
+		.name	= "atmel_nand",
 		.owner	= THIS_MODULE,
 	},
 };
 
-static int __init at91_nand_init(void)
+static int __init atmel_nand_init(void)
 {
-	return platform_driver_register(&at91_nand_driver);
+	return platform_driver_probe(&atmel_nand_driver, atmel_nand_probe);
 }
 
 
-static void __exit at91_nand_exit(void)
+static void __exit atmel_nand_exit(void)
 {
-	platform_driver_unregister(&at91_nand_driver);
+	platform_driver_unregister(&atmel_nand_driver);
 }
 
 
-module_init(at91_nand_init);
-module_exit(at91_nand_exit);
+module_init(atmel_nand_init);
+module_exit(atmel_nand_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Rick Bronson");
-MODULE_DESCRIPTION("NAND/SmartMedia driver for AT91RM9200 / AT91SAM9");
-MODULE_ALIAS("platform:at91_nand");
+MODULE_DESCRIPTION("NAND/SmartMedia driver for AT91 / AVR32");
+MODULE_ALIAS("platform:atmel_nand");
diff --git a/drivers/mtd/nand/atmel_nand_ecc.h b/drivers/mtd/nand/atmel_nand_ecc.h
new file mode 100644
index 00000000000..1ee7f993db1
--- /dev/null
+++ b/drivers/mtd/nand/atmel_nand_ecc.h
@@ -0,0 +1,36 @@
+/*
+ * Error Corrected Code Controller (ECC) - System peripherals regsters.
+ * Based on AT91SAM9260 datasheet revision B.
+ *
+ * This program is free software; 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 ATMEL_NAND_ECC_H
+#define ATMEL_NAND_ECC_H
+
+#define ATMEL_ECC_CR		0x00			/* Control register */
+#define		ATMEL_ECC_RST		(1 << 0)		/* Reset parity */
+
+#define ATMEL_ECC_MR		0x04			/* Mode register */
+#define		ATMEL_ECC_PAGESIZE	(3 << 0)		/* Page Size */
+#define			ATMEL_ECC_PAGESIZE_528		(0)
+#define			ATMEL_ECC_PAGESIZE_1056		(1)
+#define			ATMEL_ECC_PAGESIZE_2112		(2)
+#define			ATMEL_ECC_PAGESIZE_4224		(3)
+
+#define ATMEL_ECC_SR		0x08			/* Status register */
+#define		ATMEL_ECC_RECERR		(1 << 0)		/* Recoverable Error */
+#define		ATMEL_ECC_ECCERR		(1 << 1)		/* ECC Single Bit Error */
+#define		ATMEL_ECC_MULERR		(1 << 2)		/* Multiple Errors */
+
+#define ATMEL_ECC_PR		0x0c			/* Parity register */
+#define		ATMEL_ECC_BITADDR	(0xf << 0)		/* Bit Error Address */
+#define		ATMEL_ECC_WORDADDR	(0xfff << 4)		/* Word Error Address */
+
+#define ATMEL_ECC_NPR		0x10			/* NParity register */
+#define		ATMEL_ECC_NPARITY	(0xffff << 0)		/* NParity */
+
+#endif
diff --git a/drivers/mtd/nand/au1550nd.c b/drivers/mtd/nand/au1550nd.c
index 09e421a9689..761946ea45b 100644
--- a/drivers/mtd/nand/au1550nd.c
+++ b/drivers/mtd/nand/au1550nd.c
@@ -3,8 +3,6 @@
  *
  *  Copyright (C) 2004 Embedded Edge, LLC
  *
- * $Id: au1550nd.c,v 1.13 2005/11/07 11:14:30 gleixner Exp $
- *
  * 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.
@@ -604,8 +602,6 @@ module_init(au1xxx_nand_init);
  */
 static void __exit au1550_cleanup(void)
 {
-	struct nand_chip *this = (struct nand_chip *)&au1550_mtd[1];
-
 	/* Release resources, unregister device */
 	nand_release(au1550_mtd);
 
diff --git a/drivers/mtd/nand/autcpu12.c b/drivers/mtd/nand/autcpu12.c
index dd38011ee0b..553dd7e9b41 100644
--- a/drivers/mtd/nand/autcpu12.c
+++ b/drivers/mtd/nand/autcpu12.c
@@ -6,8 +6,6 @@
  *  Derived from drivers/mtd/spia.c
  *	 Copyright (C) 2000 Steven J. Hill (sjhill@realitydiluted.com)
  *
- * $Id: autcpu12.c,v 1.23 2005/11/07 11:14:30 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index da6ceaa80ba..95345d05157 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -626,10 +626,12 @@ static int __devinit cafe_nand_probe(struct pci_dev *pdev,
 {
 	struct mtd_info *mtd;
 	struct cafe_priv *cafe;
-	struct mtd_partition *parts;
 	uint32_t ctrl;
-	int nr_parts;
 	int err = 0;
+#ifdef CONFIG_MTD_PARTITIONS
+	struct mtd_partition *parts;
+	int nr_parts;
+#endif
 
 	/* Very old versions shared the same PCI ident for all three
 	   functions on the chip. Verify the class too... */
diff --git a/drivers/mtd/nand/diskonchip.c b/drivers/mtd/nand/diskonchip.c
index 0e72153b329..765d4f0f7c8 100644
--- a/drivers/mtd/nand/diskonchip.c
+++ b/drivers/mtd/nand/diskonchip.c
@@ -15,8 +15,6 @@
  * converted to the generic Reed-Solomon library by Thomas Gleixner <tglx@linutronix.de>
  *
  * Interface to generic NAND code for M-Systems DiskOnChip devices
- *
- * $Id: diskonchip.c,v 1.55 2005/11/07 11:14:30 gleixner Exp $
  */
 
 #include <linux/kernel.h>
@@ -54,8 +52,6 @@ static unsigned long __initdata doc_locations[] = {
 	0xe0000, 0xe2000, 0xe4000, 0xe6000,
 	0xe8000, 0xea000, 0xec000, 0xee000,
 #endif /*  CONFIG_MTD_DOCPROBE_HIGH */
-#elif defined(__PPC__)
-	0xe4000000,
 #else
 #warning Unknown architecture for DiskOnChip. No default probe locations defined
 #endif
diff --git a/drivers/mtd/nand/edb7312.c b/drivers/mtd/nand/edb7312.c
index ba67bbec20d..387e4352903 100644
--- a/drivers/mtd/nand/edb7312.c
+++ b/drivers/mtd/nand/edb7312.c
@@ -6,8 +6,6 @@
  *  Derived from drivers/mtd/nand/autcpu12.c
  *       Copyright (c) 2001 Thomas Gleixner (gleixner@autronix.de)
  *
- * $Id: edb7312.c,v 1.12 2005/11/07 11:14:30 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/excite_nandflash.c b/drivers/mtd/nand/excite_nandflash.c
index bed87290dec..ced14b5294d 100644
--- a/drivers/mtd/nand/excite_nandflash.c
+++ b/drivers/mtd/nand/excite_nandflash.c
@@ -209,7 +209,7 @@ static int __init excite_nand_probe(struct device *dev)
 	if (likely(!scan_res)) {
 		DEBUG(MTD_DEBUG_LEVEL2, "%s: register partitions\n", module_id);
 		add_mtd_partitions(&drvdata->board_mtd, partition_info,
-				   sizeof partition_info / sizeof partition_info[0]);
+				   ARRAY_SIZE(partition_info));
 	} else {
 		iounmap(drvdata->regs);
 		kfree(drvdata);
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index 4b69aacdf5c..9dff51351f4 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -89,7 +89,6 @@ static struct nand_ecclayout fsl_elbc_oob_sp_eccm0 = {
 	.eccbytes = 3,
 	.eccpos = {6, 7, 8},
 	.oobfree = { {0, 5}, {9, 7} },
-	.oobavail = 12,
 };
 
 /* Small Page FLASH with FMR[ECCM] = 1 */
@@ -97,7 +96,6 @@ static struct nand_ecclayout fsl_elbc_oob_sp_eccm1 = {
 	.eccbytes = 3,
 	.eccpos = {8, 9, 10},
 	.oobfree = { {0, 5}, {6, 2}, {11, 5} },
-	.oobavail = 12,
 };
 
 /* Large Page FLASH with FMR[ECCM] = 0 */
@@ -105,7 +103,6 @@ static struct nand_ecclayout fsl_elbc_oob_lp_eccm0 = {
 	.eccbytes = 12,
 	.eccpos = {6, 7, 8, 22, 23, 24, 38, 39, 40, 54, 55, 56},
 	.oobfree = { {1, 5}, {9, 13}, {25, 13}, {41, 13}, {57, 7} },
-	.oobavail = 48,
 };
 
 /* Large Page FLASH with FMR[ECCM] = 1 */
@@ -113,7 +110,48 @@ static struct nand_ecclayout fsl_elbc_oob_lp_eccm1 = {
 	.eccbytes = 12,
 	.eccpos = {8, 9, 10, 24, 25, 26, 40, 41, 42, 56, 57, 58},
 	.oobfree = { {1, 7}, {11, 13}, {27, 13}, {43, 13}, {59, 5} },
-	.oobavail = 48,
+};
+
+/*
+ * fsl_elbc_oob_lp_eccm* specify that LP NAND's OOB free area starts at offset
+ * 1, so we have to adjust bad block pattern. This pattern should be used for
+ * x8 chips only. So far hardware does not support x16 chips anyway.
+ */
+static u8 scan_ff_pattern[] = { 0xff, };
+
+static struct nand_bbt_descr largepage_memorybased = {
+	.options = 0,
+	.offs = 0,
+	.len = 1,
+	.pattern = scan_ff_pattern,
+};
+
+/*
+ * ELBC may use HW ECC, so that OOB offsets, that NAND core uses for bbt,
+ * interfere with ECC positions, that's why we implement our own descriptors.
+ * OOB {11, 5}, works for both SP and LP chips, with ECCM = 1 and ECCM = 0.
+ */
+static u8 bbt_pattern[] = {'B', 'b', 't', '0' };
+static u8 mirror_pattern[] = {'1', 't', 'b', 'B' };
+
+static struct nand_bbt_descr bbt_main_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE |
+		   NAND_BBT_2BIT | NAND_BBT_VERSION,
+	.offs =	11,
+	.len = 4,
+	.veroffs = 15,
+	.maxblocks = 4,
+	.pattern = bbt_pattern,
+};
+
+static struct nand_bbt_descr bbt_mirror_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE |
+		   NAND_BBT_2BIT | NAND_BBT_VERSION,
+	.offs =	11,
+	.len = 4,
+	.veroffs = 15,
+	.maxblocks = 4,
+	.pattern = mirror_pattern,
 };
 
 /*=================================*/
@@ -687,8 +725,7 @@ static int fsl_elbc_chip_init_tail(struct mtd_info *mtd)
 			chip->ecc.layout = (priv->fmr & FMR_ECCM) ?
 			                   &fsl_elbc_oob_lp_eccm1 :
 			                   &fsl_elbc_oob_lp_eccm0;
-			mtd->ecclayout = chip->ecc.layout;
-			mtd->oobavail = chip->ecc.layout->oobavail;
+			chip->badblock_pattern = &largepage_memorybased;
 		}
 	} else {
 		dev_err(ctrl->dev,
@@ -752,8 +789,12 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	chip->cmdfunc = fsl_elbc_cmdfunc;
 	chip->waitfunc = fsl_elbc_wait;
 
+	chip->bbt_td = &bbt_main_descr;
+	chip->bbt_md = &bbt_mirror_descr;
+
 	/* set up nand options */
-	chip->options = NAND_NO_READRDY | NAND_NO_AUTOINCR;
+	chip->options = NAND_NO_READRDY | NAND_NO_AUTOINCR |
+			NAND_USE_FLASH_BBT;
 
 	chip->controller = &ctrl->controller;
 	chip->priv = priv;
@@ -795,8 +836,8 @@ static int fsl_elbc_chip_remove(struct fsl_elbc_mtd *priv)
 	return 0;
 }
 
-static int fsl_elbc_chip_probe(struct fsl_elbc_ctrl *ctrl,
-                               struct device_node *node)
+static int __devinit fsl_elbc_chip_probe(struct fsl_elbc_ctrl *ctrl,
+					 struct device_node *node)
 {
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
 	struct fsl_elbc_mtd *priv;
@@ -917,7 +958,7 @@ static int __devinit fsl_elbc_ctrl_init(struct fsl_elbc_ctrl *ctrl)
 	return 0;
 }
 
-static int __devexit fsl_elbc_ctrl_remove(struct of_device *ofdev)
+static int fsl_elbc_ctrl_remove(struct of_device *ofdev)
 {
 	struct fsl_elbc_ctrl *ctrl = dev_get_drvdata(&ofdev->dev);
 	int i;
@@ -1041,7 +1082,7 @@ static struct of_platform_driver fsl_elbc_ctrl_driver = {
 	},
 	.match_table = fsl_elbc_match,
 	.probe = fsl_elbc_ctrl_probe,
-	.remove = __devexit_p(fsl_elbc_ctrl_remove),
+	.remove = fsl_elbc_ctrl_remove,
 };
 
 static int __init fsl_elbc_init(void)
diff --git a/drivers/mtd/nand/h1910.c b/drivers/mtd/nand/h1910.c
index 2d585d2d090..9e59de501c2 100644
--- a/drivers/mtd/nand/h1910.c
+++ b/drivers/mtd/nand/h1910.c
@@ -7,8 +7,6 @@
  *       Copyright (C) 2002 Marius Gröger (mag@sysgo.de)
  *       Copyright (c) 2001 Thomas Gleixner (gleixner@autronix.de)
  *
- * $Id: h1910.c,v 1.6 2005/11/07 11:14:30 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index ba1bdf78732..d1129bae6c2 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -798,6 +798,87 @@ static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 }
 
 /**
+ * nand_read_subpage - [REPLACABLE] software ecc based sub-page read function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @dataofs	offset of requested data within the page
+ * @readlen	data length
+ * @buf:	buffer to store read data
+ */
+static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip, uint32_t data_offs, uint32_t readlen, uint8_t *bufpoi)
+{
+	int start_step, end_step, num_steps;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+	uint8_t *p;
+	int data_col_addr, i, gaps = 0;
+	int datafrag_len, eccfrag_len, aligned_len, aligned_pos;
+	int busw = (chip->options & NAND_BUSWIDTH_16) ? 2 : 1;
+
+	/* Column address wihin the page aligned to ECC size (256bytes). */
+	start_step = data_offs / chip->ecc.size;
+	end_step = (data_offs + readlen - 1) / chip->ecc.size;
+	num_steps = end_step - start_step + 1;
+
+	/* Data size aligned to ECC ecc.size*/
+	datafrag_len = num_steps * chip->ecc.size;
+	eccfrag_len = num_steps * chip->ecc.bytes;
+
+	data_col_addr = start_step * chip->ecc.size;
+	/* If we read not a page aligned data */
+	if (data_col_addr != 0)
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, data_col_addr, -1);
+
+	p = bufpoi + data_col_addr;
+	chip->read_buf(mtd, p, datafrag_len);
+
+	/* Calculate  ECC */
+	for (i = 0; i < eccfrag_len ; i += chip->ecc.bytes, p += chip->ecc.size)
+		chip->ecc.calculate(mtd, p, &chip->buffers->ecccalc[i]);
+
+	/* The performance is faster if to position offsets
+	   according to ecc.pos. Let make sure here that
+	   there are no gaps in ecc positions */
+	for (i = 0; i < eccfrag_len - 1; i++) {
+		if (eccpos[i + start_step * chip->ecc.bytes] + 1 !=
+			eccpos[i + start_step * chip->ecc.bytes + 1]) {
+			gaps = 1;
+			break;
+		}
+	}
+	if (gaps) {
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, mtd->writesize, -1);
+		chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+	} else {
+		/* send the command to read the particular ecc bytes */
+		/* take care about buswidth alignment in read_buf */
+		aligned_pos = eccpos[start_step * chip->ecc.bytes] & ~(busw - 1);
+		aligned_len = eccfrag_len;
+		if (eccpos[start_step * chip->ecc.bytes] & (busw - 1))
+			aligned_len++;
+		if (eccpos[(start_step + num_steps) * chip->ecc.bytes] & (busw - 1))
+			aligned_len++;
+
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, mtd->writesize + aligned_pos, -1);
+		chip->read_buf(mtd, &chip->oob_poi[aligned_pos], aligned_len);
+	}
+
+	for (i = 0; i < eccfrag_len; i++)
+		chip->buffers->ecccode[i] = chip->oob_poi[eccpos[i + start_step * chip->ecc.bytes]];
+
+	p = bufpoi + data_col_addr;
+	for (i = 0; i < eccfrag_len ; i += chip->ecc.bytes, p += chip->ecc.size) {
+		int stat;
+
+		stat = chip->ecc.correct(mtd, p, &chip->buffers->ecccode[i], &chip->buffers->ecccalc[i]);
+		if (stat == -1)
+			mtd->ecc_stats.failed++;
+		else
+			mtd->ecc_stats.corrected += stat;
+	}
+	return 0;
+}
+
+/**
  * nand_read_page_hwecc - [REPLACABLE] hardware ecc based page read function
  * @mtd:	mtd info structure
  * @chip:	nand chip info structure
@@ -994,6 +1075,8 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			/* Now read the page into the buffer */
 			if (unlikely(ops->mode == MTD_OOB_RAW))
 				ret = chip->ecc.read_page_raw(mtd, chip, bufpoi);
+			else if (!aligned && NAND_SUBPAGE_READ(chip) && !oob)
+				ret = chip->ecc.read_subpage(mtd, chip, col, bytes, bufpoi);
 			else
 				ret = chip->ecc.read_page(mtd, chip, bufpoi);
 			if (ret < 0)
@@ -1001,7 +1084,8 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 
 			/* Transfer not aligned data */
 			if (!aligned) {
-				chip->pagebuf = realpage;
+				if (!NAND_SUBPAGE_READ(chip) && !oob)
+					chip->pagebuf = realpage;
 				memcpy(buf, chip->buffers->databuf + col, bytes);
 			}
 
@@ -2521,6 +2605,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 		chip->ecc.calculate = nand_calculate_ecc;
 		chip->ecc.correct = nand_correct_data;
 		chip->ecc.read_page = nand_read_page_swecc;
+		chip->ecc.read_subpage = nand_read_subpage;
 		chip->ecc.write_page = nand_write_page_swecc;
 		chip->ecc.read_oob = nand_read_oob_std;
 		chip->ecc.write_oob = nand_write_oob_std;
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 5e121ceaa59..0b1c48595f1 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -6,8 +6,6 @@
  *
  *  Copyright (C) 2004 Thomas Gleixner (tglx@linutronix.de)
  *
- * $Id: nand_bbt.c,v 1.36 2005/11/07 11:14:30 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/nand_ecc.c b/drivers/mtd/nand/nand_ecc.c
index 9003a135e05..918a806a847 100644
--- a/drivers/mtd/nand/nand_ecc.c
+++ b/drivers/mtd/nand/nand_ecc.c
@@ -9,8 +9,6 @@
  *
  * Copyright (C) 2006 Thomas Gleixner <tglx@linutronix.de>
  *
- * $Id: nand_ecc.c,v 1.15 2005/11/07 11:14:30 gleixner Exp $
- *
  * This file is free software; 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
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index a3e3ab0185d..69ee2c90eb0 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -3,8 +3,6 @@
  *
  *  Copyright (C) 2002 Thomas Gleixner (tglx@linutronix.de)
  *
- * $Id: nand_ids.c,v 1.16 2005/11/07 11:14:31 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index bb885d1fcab..ecd70e2504f 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -21,8 +21,6 @@
  * 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
- *
- * $Id: nandsim.c,v 1.8 2005/03/19 15:33:56 dedekind Exp $
  */
 
 #include <linux/init.h>
@@ -39,6 +37,7 @@
 #include <linux/delay.h>
 #include <linux/list.h>
 #include <linux/random.h>
+#include <asm/div64.h>
 
 /* Default simulator parameters values */
 #if !defined(CONFIG_NANDSIM_FIRST_ID_BYTE)  || \
@@ -298,11 +297,11 @@ struct nandsim {
 
 	/* NAND flash "geometry" */
 	struct nandsin_geometry {
-		uint32_t totsz;     /* total flash size, bytes */
+		uint64_t totsz;     /* total flash size, bytes */
 		uint32_t secsz;     /* flash sector (erase block) size, bytes */
 		uint pgsz;          /* NAND flash page size, bytes */
 		uint oobsz;         /* page OOB area size, bytes */
-		uint32_t totszoob;  /* total flash size including OOB, bytes */
+		uint64_t totszoob;  /* total flash size including OOB, bytes */
 		uint pgszoob;       /* page size including OOB , bytes*/
 		uint secszoob;      /* sector size including OOB, bytes */
 		uint pgnum;         /* total number of pages */
@@ -459,6 +458,12 @@ static char *get_partition_name(int i)
 	return kstrdup(buf, GFP_KERNEL);
 }
 
+static u_int64_t divide(u_int64_t n, u_int32_t d)
+{
+	do_div(n, d);
+	return n;
+}
+
 /*
  * Initialize the nandsim structure.
  *
@@ -469,8 +474,8 @@ static int init_nandsim(struct mtd_info *mtd)
 	struct nand_chip *chip = (struct nand_chip *)mtd->priv;
 	struct nandsim   *ns   = (struct nandsim *)(chip->priv);
 	int i, ret = 0;
-	u_int32_t remains;
-	u_int32_t next_offset;
+	u_int64_t remains;
+	u_int64_t next_offset;
 
 	if (NS_IS_INITIALIZED(ns)) {
 		NS_ERR("init_nandsim: nandsim is already initialized\n");
@@ -487,8 +492,8 @@ static int init_nandsim(struct mtd_info *mtd)
 	ns->geom.oobsz    = mtd->oobsize;
 	ns->geom.secsz    = mtd->erasesize;
 	ns->geom.pgszoob  = ns->geom.pgsz + ns->geom.oobsz;
-	ns->geom.pgnum    = ns->geom.totsz / ns->geom.pgsz;
-	ns->geom.totszoob = ns->geom.totsz + ns->geom.pgnum * ns->geom.oobsz;
+	ns->geom.pgnum    = divide(ns->geom.totsz, ns->geom.pgsz);
+	ns->geom.totszoob = ns->geom.totsz + (uint64_t)ns->geom.pgnum * ns->geom.oobsz;
 	ns->geom.secshift = ffs(ns->geom.secsz) - 1;
 	ns->geom.pgshift  = chip->page_shift;
 	ns->geom.oobshift = ffs(ns->geom.oobsz) - 1;
@@ -511,7 +516,7 @@ static int init_nandsim(struct mtd_info *mtd)
 	}
 
 	if (ns->options & OPT_SMALLPAGE) {
-		if (ns->geom.totsz < (32 << 20)) {
+		if (ns->geom.totsz <= (32 << 20)) {
 			ns->geom.pgaddrbytes  = 3;
 			ns->geom.secaddrbytes = 2;
 		} else {
@@ -537,15 +542,16 @@ static int init_nandsim(struct mtd_info *mtd)
 	remains = ns->geom.totsz;
 	next_offset = 0;
 	for (i = 0; i < parts_num; ++i) {
-		unsigned long part = parts[i];
-		if (!part || part > remains / ns->geom.secsz) {
+		u_int64_t part_sz = (u_int64_t)parts[i] * ns->geom.secsz;
+
+		if (!part_sz || part_sz > remains) {
 			NS_ERR("bad partition size.\n");
 			ret = -EINVAL;
 			goto error;
 		}
 		ns->partitions[i].name   = get_partition_name(i);
 		ns->partitions[i].offset = next_offset;
-		ns->partitions[i].size   = part * ns->geom.secsz;
+		ns->partitions[i].size   = part_sz;
 		next_offset += ns->partitions[i].size;
 		remains -= ns->partitions[i].size;
 	}
@@ -573,7 +579,7 @@ static int init_nandsim(struct mtd_info *mtd)
 	if (ns->busw == 16)
 		NS_WARN("16-bit flashes support wasn't tested\n");
 
-	printk("flash size: %u MiB\n",          ns->geom.totsz >> 20);
+	printk("flash size: %llu MiB\n",        ns->geom.totsz >> 20);
 	printk("page size: %u bytes\n",         ns->geom.pgsz);
 	printk("OOB area size: %u bytes\n",     ns->geom.oobsz);
 	printk("sector size: %u KiB\n",         ns->geom.secsz >> 10);
@@ -583,7 +589,7 @@ static int init_nandsim(struct mtd_info *mtd)
 	printk("bits in sector size: %u\n",     ns->geom.secshift);
 	printk("bits in page size: %u\n",       ns->geom.pgshift);
 	printk("bits in OOB size: %u\n",        ns->geom.oobshift);
-	printk("flash size with OOB: %u KiB\n", ns->geom.totszoob >> 10);
+	printk("flash size with OOB: %llu KiB\n", ns->geom.totszoob >> 10);
 	printk("page address bytes: %u\n",      ns->geom.pgaddrbytes);
 	printk("sector address bytes: %u\n",    ns->geom.secaddrbytes);
 	printk("options: %#x\n",                ns->options);
@@ -825,7 +831,7 @@ static int setup_wear_reporting(struct mtd_info *mtd)
 
 	if (!rptwear)
 		return 0;
-	wear_eb_count = mtd->size / mtd->erasesize;
+	wear_eb_count = divide(mtd->size, mtd->erasesize);
 	mem = wear_eb_count * sizeof(unsigned long);
 	if (mem / sizeof(unsigned long) != wear_eb_count) {
 		NS_ERR("Too many erase blocks for wear reporting\n");
@@ -2013,7 +2019,7 @@ static int __init ns_init_module(void)
 	}
 
 	if (overridesize) {
-		u_int32_t new_size = nsmtd->erasesize << overridesize;
+		u_int64_t new_size = (u_int64_t)nsmtd->erasesize << overridesize;
 		if (new_size >> overridesize != nsmtd->erasesize) {
 			NS_ERR("overridesize is too big\n");
 			goto err_exit;
@@ -2021,7 +2027,8 @@ static int __init ns_init_module(void)
 		/* N.B. This relies on nand_scan not doing anything with the size before we change it */
 		nsmtd->size = new_size;
 		chip->chipsize = new_size;
-		chip->chip_shift = ffs(new_size) - 1;
+		chip->chip_shift = ffs(nsmtd->erasesize) + overridesize - 1;
+		chip->pagemask = (chip->chipsize >> chip->page_shift) - 1;
 	}
 
 	if ((retval = setup_wear_reporting(nsmtd)) != 0)
diff --git a/drivers/mtd/nand/ppchameleonevb.c b/drivers/mtd/nand/ppchameleonevb.c
index 082073acf20..cc865843185 100644
--- a/drivers/mtd/nand/ppchameleonevb.c
+++ b/drivers/mtd/nand/ppchameleonevb.c
@@ -6,8 +6,6 @@
  *  Derived from drivers/mtd/nand/edb7312.c
  *
  *
- * $Id: ppchameleonevb.c,v 1.7 2005/11/07 11:14:31 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/rtc_from4.c b/drivers/mtd/nand/rtc_from4.c
index 26f88215bc4..a033c4cd8e1 100644
--- a/drivers/mtd/nand/rtc_from4.c
+++ b/drivers/mtd/nand/rtc_from4.c
@@ -6,8 +6,6 @@
  *  Derived from drivers/mtd/nand/spia.c
  *       Copyright (C) 2000 Steven J. Hill (sjhill@realitydiluted.com)
  *
- * $Id: rtc_from4.c,v 1.10 2005/11/07 11:14:31 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index b34a460ab67..556139ed1fd 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -1,26 +1,10 @@
 /* linux/drivers/mtd/nand/s3c2410.c
  *
- * Copyright (c) 2004,2005 Simtec Electronics
- *	http://www.simtec.co.uk/products/SWLINUX/
+ * Copyright © 2004-2008 Simtec Electronics
+ *	http://armlinux.simtec.co.uk/
  *	Ben Dooks <ben@simtec.co.uk>
  *
- * Samsung S3C2410/S3C240 NAND driver
- *
- * Changelog:
- *	21-Sep-2004  BJD  Initial version
- *	23-Sep-2004  BJD  Multiple device support
- *	28-Sep-2004  BJD  Fixed ECC placement for Hardware mode
- *	12-Oct-2004  BJD  Fixed errors in use of platform data
- *	18-Feb-2005  BJD  Fix sparse errors
- *	14-Mar-2005  BJD  Applied tglx's code reduction patch
- *	02-May-2005  BJD  Fixed s3c2440 support
- *	02-May-2005  BJD  Reduced hwcontrol decode
- *	20-Jun-2005  BJD  Updated s3c2440 support, fixed timing bug
- *	08-Jul-2005  BJD  Fix OOPS when no platform data supplied
- *	20-Oct-2005  BJD  Fix timing calculation bug
- *	14-Jan-2006  BJD  Allow clock to be stopped when idle
- *
- * $Id: s3c2410.c,v 1.23 2006/04/01 18:06:29 bjd Exp $
+ * Samsung S3C2410/S3C2440/S3C2412 NAND driver
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -52,6 +36,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/cpufreq.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -120,8 +105,13 @@ struct s3c2410_nand_info {
 	int				sel_bit;
 	int				mtd_count;
 	unsigned long			save_sel;
+	unsigned long			clk_rate;
 
 	enum s3c_cpu_type		cpu_type;
+
+#ifdef CONFIG_CPU_FREQ
+	struct notifier_block	freq_transition;
+#endif
 };
 
 /* conversion functions */
@@ -179,17 +169,18 @@ static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 
 /* controller setup */
 
-static int s3c2410_nand_inithw(struct s3c2410_nand_info *info,
-			       struct platform_device *pdev)
+static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 {
-	struct s3c2410_platform_nand *plat = to_nand_plat(pdev);
-	unsigned long clkrate = clk_get_rate(info->clk);
+	struct s3c2410_platform_nand *plat = info->platform;
 	int tacls_max = (info->cpu_type == TYPE_S3C2412) ? 8 : 4;
 	int tacls, twrph0, twrph1;
-	unsigned long cfg = 0;
+	unsigned long clkrate = clk_get_rate(info->clk);
+	unsigned long set, cfg, mask;
+	unsigned long flags;
 
 	/* calculate the timing information for the controller */
 
+	info->clk_rate = clkrate;
 	clkrate /= 1000;	/* turn clock into kHz for ease of use */
 
 	if (plat != NULL) {
@@ -211,28 +202,69 @@ static int s3c2410_nand_inithw(struct s3c2410_nand_info *info,
 	dev_info(info->device, "Tacls=%d, %dns Twrph0=%d %dns, Twrph1=%d %dns\n",
 	       tacls, to_ns(tacls, clkrate), twrph0, to_ns(twrph0, clkrate), twrph1, to_ns(twrph1, clkrate));
 
+	switch (info->cpu_type) {
+	case TYPE_S3C2410:
+		mask = (S3C2410_NFCONF_TACLS(3) |
+			S3C2410_NFCONF_TWRPH0(7) |
+			S3C2410_NFCONF_TWRPH1(7));
+		set = S3C2410_NFCONF_EN;
+		set |= S3C2410_NFCONF_TACLS(tacls - 1);
+		set |= S3C2410_NFCONF_TWRPH0(twrph0 - 1);
+		set |= S3C2410_NFCONF_TWRPH1(twrph1 - 1);
+		break;
+
+	case TYPE_S3C2440:
+	case TYPE_S3C2412:
+		mask = (S3C2410_NFCONF_TACLS(tacls_max - 1) |
+			S3C2410_NFCONF_TWRPH0(7) |
+			S3C2410_NFCONF_TWRPH1(7));
+
+		set = S3C2440_NFCONF_TACLS(tacls - 1);
+		set |= S3C2440_NFCONF_TWRPH0(twrph0 - 1);
+		set |= S3C2440_NFCONF_TWRPH1(twrph1 - 1);
+		break;
+
+	default:
+		/* keep compiler happy */
+		mask = 0;
+		set = 0;
+		BUG();
+	}
+
+	dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
+
+	local_irq_save(flags);
+
+	cfg = readl(info->regs + S3C2410_NFCONF);
+	cfg &= ~mask;
+	cfg |= set;
+	writel(cfg, info->regs + S3C2410_NFCONF);
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static int s3c2410_nand_inithw(struct s3c2410_nand_info *info)
+{
+	int ret;
+
+	ret = s3c2410_nand_setrate(info);
+	if (ret < 0)
+		return ret;
+
  	switch (info->cpu_type) {
  	case TYPE_S3C2410:
-		cfg = S3C2410_NFCONF_EN;
-		cfg |= S3C2410_NFCONF_TACLS(tacls - 1);
-		cfg |= S3C2410_NFCONF_TWRPH0(twrph0 - 1);
-		cfg |= S3C2410_NFCONF_TWRPH1(twrph1 - 1);
+	default:
 		break;
 
  	case TYPE_S3C2440:
  	case TYPE_S3C2412:
-		cfg = S3C2440_NFCONF_TACLS(tacls - 1);
-		cfg |= S3C2440_NFCONF_TWRPH0(twrph0 - 1);
-		cfg |= S3C2440_NFCONF_TWRPH1(twrph1 - 1);
-
 		/* enable the controller and de-assert nFCE */
 
 		writel(S3C2440_NFCONT_ENABLE, info->regs + S3C2440_NFCONT);
 	}
 
-	dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
-
-	writel(cfg, info->regs + S3C2410_NFCONF);
 	return 0;
 }
 
@@ -513,6 +545,52 @@ static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int
 	writesl(info->regs + S3C2440_NFDATA, buf, len / 4);
 }
 
+/* cpufreq driver support */
+
+#ifdef CONFIG_CPU_FREQ
+
+static int s3c2410_nand_cpufreq_transition(struct notifier_block *nb,
+					  unsigned long val, void *data)
+{
+	struct s3c2410_nand_info *info;
+	unsigned long newclk;
+
+	info = container_of(nb, struct s3c2410_nand_info, freq_transition);
+	newclk = clk_get_rate(info->clk);
+
+	if ((val == CPUFREQ_POSTCHANGE && newclk < info->clk_rate) ||
+	    (val == CPUFREQ_PRECHANGE && newclk > info->clk_rate)) {
+		s3c2410_nand_setrate(info);
+	}
+
+	return 0;
+}
+
+static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
+{
+	info->freq_transition.notifier_call = s3c2410_nand_cpufreq_transition;
+
+	return cpufreq_register_notifier(&info->freq_transition,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
+{
+	cpufreq_unregister_notifier(&info->freq_transition,
+				    CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+#else
+static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
+{
+	return 0;
+}
+
+static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
+{
+}
+#endif
+
 /* device management functions */
 
 static int s3c2410_nand_remove(struct platform_device *pdev)
@@ -524,9 +602,10 @@ static int s3c2410_nand_remove(struct platform_device *pdev)
 	if (info == NULL)
 		return 0;
 
-	/* first thing we need to do is release all our mtds
-	 * and their partitions, then go through freeing the
-	 * resources used
+	s3c2410_nand_cpufreq_deregister(info);
+
+	/* Release all our mtds  and their partitions, then go through
+	 * freeing the resources used
 	 */
 
 	if (info->mtds != NULL) {
@@ -691,7 +770,8 @@ static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
 {
 	struct nand_chip *chip = &nmtd->chip;
 
-	printk("%s: chip %p: %d\n", __func__, chip, chip->page_shift);
+	dev_dbg(info->device, "chip %p => page shift %d\n",
+		chip, chip->page_shift);
 
 	if (hardware_ecc) {
 		/* change the behaviour depending on wether we are using
@@ -784,7 +864,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 
 	/* initialise the hardware */
 
-	err = s3c2410_nand_inithw(info, pdev);
+	err = s3c2410_nand_inithw(info);
 	if (err != 0)
 		goto exit_error;
 
@@ -827,6 +907,12 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 			sets++;
 	}
 
+	err = s3c2410_nand_cpufreq_register(info);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to init cpufreq support\n");
+		goto exit_error;
+	}
+
 	if (allow_clk_stop(info)) {
 		dev_info(&pdev->dev, "clock idle support enabled\n");
 		clk_disable(info->clk);
@@ -874,7 +960,7 @@ static int s3c24xx_nand_resume(struct platform_device *dev)
 
 	if (info) {
 		clk_enable(info->clk);
-		s3c2410_nand_inithw(info, dev);
+		s3c2410_nand_inithw(info);
 
 		/* Restore the state of the nFCE line. */
 
diff --git a/drivers/mtd/nand/sharpsl.c b/drivers/mtd/nand/sharpsl.c
index 033f8800b1e..6dba2fb66ae 100644
--- a/drivers/mtd/nand/sharpsl.c
+++ b/drivers/mtd/nand/sharpsl.c
@@ -3,8 +3,6 @@
  *
  *  Copyright (C) 2004 Richard Purdie
  *
- *  $Id: sharpsl.c,v 1.7 2005/11/07 11:14:31 gleixner Exp $
- *
  *  Based on Sharp's NAND driver sharp_sl.c
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/mtd/nand/spia.c b/drivers/mtd/nand/spia.c
index 1f6d429b158..0cc6d0acb8f 100644
--- a/drivers/mtd/nand/spia.c
+++ b/drivers/mtd/nand/spia.c
@@ -8,8 +8,6 @@
  *			to controllines	(due to change in nand.c)
  *			page_cache added
  *
- * $Id: spia.c,v 1.25 2005/11/07 11:14:31 gleixner Exp $
- *
  * 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.
diff --git a/drivers/mtd/nand/toto.c b/drivers/mtd/nand/toto.c
index f9e2d4a0ab8..bbf492e6830 100644
--- a/drivers/mtd/nand/toto.c
+++ b/drivers/mtd/nand/toto.c
@@ -14,8 +14,6 @@
  *  Overview:
  *   This is a device driver for the NAND flash device found on the
  *   TI fido board. It supports 32MiB and 64MiB cards
- *
- * $Id: toto.c,v 1.5 2005/11/07 11:14:31 gleixner Exp $
  */
 
 #include <linux/slab.h>
diff --git a/drivers/mtd/nand/ts7250.c b/drivers/mtd/nand/ts7250.c
index f40081069ab..807a72752ee 100644
--- a/drivers/mtd/nand/ts7250.c
+++ b/drivers/mtd/nand/ts7250.c
@@ -9,8 +9,6 @@
  * Derived from drivers/mtd/nand/autcpu12.c
  *   Copyright (c) 2001 Thomas Gleixner (gleixner@autronix.de)
  *
- * $Id: ts7250.c,v 1.4 2004/12/30 22:02:07 joff Exp $
- *
  * 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.
diff --git a/drivers/mtd/nftlcore.c b/drivers/mtd/nftlcore.c
index 0c9ce19ea27..320b929abe7 100644
--- a/drivers/mtd/nftlcore.c
+++ b/drivers/mtd/nftlcore.c
@@ -1,7 +1,6 @@
 /* Linux driver for NAND Flash Translation Layer      */
 /* (c) 1999 Machine Vision Holdings, Inc.             */
 /* Author: David Woodhouse <dwmw2@infradead.org>      */
-/* $Id: nftlcore.c,v 1.98 2005/11/07 11:14:21 gleixner Exp $ */
 
 /*
   The contents of this file are distributed under the GNU General
@@ -803,12 +802,8 @@ static struct mtd_blktrans_ops nftl_tr = {
 	.owner		= THIS_MODULE,
 };
 
-extern char nftlmountrev[];
-
 static int __init init_nftl(void)
 {
-	printk(KERN_INFO "NFTL driver: nftlcore.c $Revision: 1.98 $, nftlmount.c %s\n", nftlmountrev);
-
 	return register_mtd_blktrans(&nftl_tr);
 }
 
diff --git a/drivers/mtd/nftlmount.c b/drivers/mtd/nftlmount.c
index 345e6eff89c..ccc4f209fbb 100644
--- a/drivers/mtd/nftlmount.c
+++ b/drivers/mtd/nftlmount.c
@@ -4,8 +4,6 @@
  * Author: Fabrice Bellard (fabrice.bellard@netgem.com)
  * Copyright (C) 2000 Netgem S.A.
  *
- * $Id: nftlmount.c,v 1.41 2005/11/07 11:14:21 gleixner Exp $
- *
  * This program is free software; 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
@@ -31,8 +29,6 @@
 
 #define SECTORSIZE 512
 
-char nftlmountrev[]="$Revision: 1.41 $";
-
 /* find_boot_record: Find the NFTL Media Header and its Spare copy which contains the
  *	various device information of the NFTL partition and Bad Unit Table. Update
  *	the ReplUnitTable[] table accroding to the Bad Unit Table. ReplUnitTable[]
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index 5d7965f7e9c..926cf3a4135 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -325,28 +325,11 @@ static int onenand_wait(struct mtd_info *mtd, int state)
 
 	ctrl = this->read_word(this->base + ONENAND_REG_CTRL_STATUS);
 
-	if (ctrl & ONENAND_CTRL_ERROR) {
-		printk(KERN_ERR "onenand_wait: controller error = 0x%04x\n", ctrl);
-		if (ctrl & ONENAND_CTRL_LOCK)
-			printk(KERN_ERR "onenand_wait: it's locked error.\n");
-		if (state == FL_READING) {
-			/*
-			 * A power loss while writing can result in a page
-			 * becoming unreadable.  When the device is mounted
-			 * again, reading that page gives controller errors.
-			 * Upper level software like JFFS2 treat -EIO as fatal,
-			 * refusing to mount at all.  That means it is necessary
-			 * to treat the error as an ECC error to allow recovery.
-			 * Note that typically in this case, the eraseblock can
-			 * still be erased and rewritten i.e. it has not become
-			 * a bad block.
-			 */
-			mtd->ecc_stats.failed++;
-			return -EBADMSG;
-		}
-		return -EIO;
-	}
-
+	/*
+	 * In the Spec. it checks the controller status first
+	 * However if you get the correct information in case of
+	 * power off recovery (POR) test, it should read ECC status first
+	 */
 	if (interrupt & ONENAND_INT_READ) {
 		int ecc = this->read_word(this->base + ONENAND_REG_ECC_STATUS);
 		if (ecc) {
@@ -364,6 +347,15 @@ static int onenand_wait(struct mtd_info *mtd, int state)
 		return -EIO;
 	}
 
+	/* If there's controller error, it's a real error */
+	if (ctrl & ONENAND_CTRL_ERROR) {
+		printk(KERN_ERR "onenand_wait: controller error = 0x%04x\n",
+			ctrl);
+		if (ctrl & ONENAND_CTRL_LOCK)
+			printk(KERN_ERR "onenand_wait: it's locked error.\n");
+		return -EIO;
+	}
+
 	return 0;
 }
 
@@ -1135,22 +1127,26 @@ static int onenand_bbt_wait(struct mtd_info *mtd, int state)
 	interrupt = this->read_word(this->base + ONENAND_REG_INTERRUPT);
 	ctrl = this->read_word(this->base + ONENAND_REG_CTRL_STATUS);
 
-	/* Initial bad block case: 0x2400 or 0x0400 */
-	if (ctrl & ONENAND_CTRL_ERROR) {
-		printk(KERN_DEBUG "onenand_bbt_wait: controller error = 0x%04x\n", ctrl);
-		return ONENAND_BBT_READ_ERROR;
-	}
-
 	if (interrupt & ONENAND_INT_READ) {
 		int ecc = this->read_word(this->base + ONENAND_REG_ECC_STATUS);
-		if (ecc & ONENAND_ECC_2BIT_ALL)
+		if (ecc & ONENAND_ECC_2BIT_ALL) {
+			printk(KERN_INFO "onenand_bbt_wait: ecc error = 0x%04x"
+				", controller error 0x%04x\n", ecc, ctrl);
 			return ONENAND_BBT_READ_ERROR;
+		}
 	} else {
 		printk(KERN_ERR "onenand_bbt_wait: read timeout!"
 			"ctrl=0x%04x intr=0x%04x\n", ctrl, interrupt);
 		return ONENAND_BBT_READ_FATAL_ERROR;
 	}
 
+	/* Initial bad block case: 0x2400 or 0x0400 */
+	if (ctrl & ONENAND_CTRL_ERROR) {
+		printk(KERN_DEBUG "onenand_bbt_wait: "
+			"controller error = 0x%04x\n", ctrl);
+		return ONENAND_BBT_READ_ERROR;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/mtd/redboot.c b/drivers/mtd/redboot.c
index c5030f94f04..2d600a1bf2a 100644
--- a/drivers/mtd/redboot.c
+++ b/drivers/mtd/redboot.c
@@ -1,6 +1,4 @@
 /*
- * $Id: redboot.c,v 1.21 2006/03/30 18:34:37 bjd Exp $
- *
  * Parse RedBoot-style Flash Image System (FIS) tables and
  * produce a Linux partition array to match.
  */
diff --git a/drivers/mtd/rfd_ftl.c b/drivers/mtd/rfd_ftl.c
index c84e4546549..e538c0a72ab 100644
--- a/drivers/mtd/rfd_ftl.c
+++ b/drivers/mtd/rfd_ftl.c
@@ -3,8 +3,6 @@
  *
  * Copyright (C) 2005  Sean Young <sean@mess.org>
  *
- * $Id: rfd_ftl.c,v 1.8 2006/01/15 12:51:44 sean Exp $
- *
  * This type of flash translation layer (FTL) is used by the Embedded BIOS
  * by General Software. It is known as the Resident Flash Disk (RFD), see:
  *
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 961416ac061..c7630a22831 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -51,14 +51,13 @@
  * @name: MTD device name or number string
  * @vid_hdr_offs: VID header offset
  */
-struct mtd_dev_param
-{
+struct mtd_dev_param {
 	char name[MTD_PARAM_LEN_MAX];
 	int vid_hdr_offs;
 };
 
 /* Numbers of elements set in the @mtd_dev_param array */
-static int mtd_devs = 0;
+static int mtd_devs;
 
 /* MTD devices specification parameters */
 static struct mtd_dev_param mtd_dev_param[UBI_MAX_DEVICES];
@@ -160,8 +159,7 @@ void ubi_put_device(struct ubi_device *ubi)
 }
 
 /**
- * ubi_get_by_major - get UBI device description object by character device
- *                    major number.
+ * ubi_get_by_major - get UBI device by character device major number.
  * @major: major number
  *
  * This function is similar to 'ubi_get_device()', but it searches the device
@@ -355,15 +353,34 @@ static void kill_volumes(struct ubi_device *ubi)
 }
 
 /**
+ * free_user_volumes - free all user volumes.
+ * @ubi: UBI device description object
+ *
+ * Normally the volumes are freed at the release function of the volume device
+ * objects. However, on error paths the volumes have to be freed before the
+ * device objects have been initialized.
+ */
+static void free_user_volumes(struct ubi_device *ubi)
+{
+	int i;
+
+	for (i = 0; i < ubi->vtbl_slots; i++)
+		if (ubi->volumes[i]) {
+			kfree(ubi->volumes[i]->eba_tbl);
+			kfree(ubi->volumes[i]);
+		}
+}
+
+/**
  * uif_init - initialize user interfaces for an UBI device.
  * @ubi: UBI device description object
  *
  * This function returns zero in case of success and a negative error code in
- * case of failure.
+ * case of failure. Note, this function destroys all volumes if it failes.
  */
 static int uif_init(struct ubi_device *ubi)
 {
-	int i, err;
+	int i, err, do_free = 0;
 	dev_t dev;
 
 	sprintf(ubi->ubi_name, UBI_NAME_STR "%d", ubi->ubi_num);
@@ -384,7 +401,7 @@ static int uif_init(struct ubi_device *ubi)
 
 	ubi_assert(MINOR(dev) == 0);
 	cdev_init(&ubi->cdev, &ubi_cdev_operations);
-	dbg_msg("%s major is %u", ubi->ubi_name, MAJOR(dev));
+	dbg_gen("%s major is %u", ubi->ubi_name, MAJOR(dev));
 	ubi->cdev.owner = THIS_MODULE;
 
 	err = cdev_add(&ubi->cdev, dev, 1);
@@ -410,10 +427,13 @@ static int uif_init(struct ubi_device *ubi)
 
 out_volumes:
 	kill_volumes(ubi);
+	do_free = 0;
 out_sysfs:
 	ubi_sysfs_close(ubi);
 	cdev_del(&ubi->cdev);
 out_unreg:
+	if (do_free)
+		free_user_volumes(ubi);
 	unregister_chrdev_region(ubi->cdev.dev, ubi->vtbl_slots + 1);
 	ubi_err("cannot initialize UBI %s, error %d", ubi->ubi_name, err);
 	return err;
@@ -422,6 +442,10 @@ out_unreg:
 /**
  * uif_close - close user interfaces for an UBI device.
  * @ubi: UBI device description object
+ *
+ * Note, since this function un-registers UBI volume device objects (@vol->dev),
+ * the memory allocated voe the volumes is freed as well (in the release
+ * function).
  */
 static void uif_close(struct ubi_device *ubi)
 {
@@ -432,6 +456,21 @@ static void uif_close(struct ubi_device *ubi)
 }
 
 /**
+ * free_internal_volumes - free internal volumes.
+ * @ubi: UBI device description object
+ */
+static void free_internal_volumes(struct ubi_device *ubi)
+{
+	int i;
+
+	for (i = ubi->vtbl_slots;
+	     i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++) {
+		kfree(ubi->volumes[i]->eba_tbl);
+		kfree(ubi->volumes[i]);
+	}
+}
+
+/**
  * attach_by_scanning - attach an MTD device using scanning method.
  * @ubi: UBI device descriptor
  *
@@ -475,6 +514,7 @@ static int attach_by_scanning(struct ubi_device *ubi)
 out_wl:
 	ubi_wl_close(ubi);
 out_vtbl:
+	free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 out_si:
 	ubi_scan_destroy_si(si);
@@ -482,7 +522,7 @@ out_si:
 }
 
 /**
- * io_init - initialize I/O unit for a given UBI device.
+ * io_init - initialize I/O sub-system for a given UBI device.
  * @ubi: UBI device description object
  *
  * If @ubi->vid_hdr_offset or @ubi->leb_start is zero, default offsets are
@@ -530,7 +570,11 @@ static int io_init(struct ubi_device *ubi)
 	ubi->min_io_size = ubi->mtd->writesize;
 	ubi->hdrs_min_io_size = ubi->mtd->writesize >> ubi->mtd->subpage_sft;
 
-	/* Make sure minimal I/O unit is power of 2 */
+	/*
+	 * Make sure minimal I/O unit is power of 2. Note, there is no
+	 * fundamental reason for this assumption. It is just an optimization
+	 * which allows us to avoid costly division operations.
+	 */
 	if (!is_power_of_2(ubi->min_io_size)) {
 		ubi_err("min. I/O unit (%d) is not power of 2",
 			ubi->min_io_size);
@@ -581,7 +625,7 @@ static int io_init(struct ubi_device *ubi)
 	if (ubi->vid_hdr_offset < UBI_EC_HDR_SIZE ||
 	    ubi->leb_start < ubi->vid_hdr_offset + UBI_VID_HDR_SIZE ||
 	    ubi->leb_start > ubi->peb_size - UBI_VID_HDR_SIZE ||
-	    ubi->leb_start % ubi->min_io_size) {
+	    ubi->leb_start & (ubi->min_io_size - 1)) {
 		ubi_err("bad VID header (%d) or data offsets (%d)",
 			ubi->vid_hdr_offset, ubi->leb_start);
 		return -EINVAL;
@@ -646,7 +690,7 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 
 	/*
 	 * Clear the auto-resize flag in the volume in-memory copy of the
-	 * volume table, and 'ubi_resize_volume()' will propogate this change
+	 * volume table, and 'ubi_resize_volume()' will propagate this change
 	 * to the flash.
 	 */
 	ubi->vtbl[vol_id].flags &= ~UBI_VTBL_AUTORESIZE_FLG;
@@ -655,7 +699,7 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 		struct ubi_vtbl_record vtbl_rec;
 
 		/*
-		 * No avalilable PEBs to re-size the volume, clear the flag on
+		 * No available PEBs to re-size the volume, clear the flag on
 		 * flash and exit.
 		 */
 		memcpy(&vtbl_rec, &ubi->vtbl[vol_id],
@@ -682,13 +726,13 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 
 /**
  * ubi_attach_mtd_dev - attach an MTD device.
- * @mtd_dev: MTD device description object
+ * @mtd: MTD device description object
  * @ubi_num: number to assign to the new UBI device
  * @vid_hdr_offset: VID header offset
  *
  * This function attaches MTD device @mtd_dev to UBI and assign @ubi_num number
  * to the newly created UBI device, unless @ubi_num is %UBI_DEV_NUM_AUTO, in
- * which case this function finds a vacant device nubert and assings it
+ * which case this function finds a vacant device number and assigns it
  * automatically. Returns the new UBI device number in case of success and a
  * negative error code in case of failure.
  *
@@ -698,7 +742,7 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 {
 	struct ubi_device *ubi;
-	int i, err;
+	int i, err, do_free = 1;
 
 	/*
 	 * Check if we already have the same MTD device attached.
@@ -735,7 +779,8 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 			if (!ubi_devices[ubi_num])
 				break;
 		if (ubi_num == UBI_MAX_DEVICES) {
-			dbg_err("only %d UBI devices may be created", UBI_MAX_DEVICES);
+			dbg_err("only %d UBI devices may be created",
+				UBI_MAX_DEVICES);
 			return -ENFILE;
 		}
 	} else {
@@ -760,6 +805,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 
 	mutex_init(&ubi->buf_mutex);
 	mutex_init(&ubi->ckvol_mutex);
+	mutex_init(&ubi->mult_mutex);
 	mutex_init(&ubi->volumes_mutex);
 	spin_lock_init(&ubi->volumes_lock);
 
@@ -798,7 +844,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 
 	err = uif_init(ubi);
 	if (err)
-		goto out_detach;
+		goto out_nofree;
 
 	ubi->bgt_thread = kthread_create(ubi_thread, ubi, ubi->bgt_name);
 	if (IS_ERR(ubi->bgt_thread)) {
@@ -824,20 +870,22 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 		ubi->beb_rsvd_pebs);
 	ubi_msg("max/mean erase counter: %d/%d", ubi->max_ec, ubi->mean_ec);
 
-	/* Enable the background thread */
-	if (!DBG_DISABLE_BGT) {
+	if (!DBG_DISABLE_BGT)
 		ubi->thread_enabled = 1;
-		wake_up_process(ubi->bgt_thread);
-	}
+	wake_up_process(ubi->bgt_thread);
 
 	ubi_devices[ubi_num] = ubi;
 	return ubi_num;
 
 out_uif:
 	uif_close(ubi);
+out_nofree:
+	do_free = 0;
 out_detach:
-	ubi_eba_close(ubi);
 	ubi_wl_close(ubi);
+	if (do_free)
+		free_user_volumes(ubi);
+	free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 out_free:
 	vfree(ubi->peb_buf1);
@@ -899,8 +947,8 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 		kthread_stop(ubi->bgt_thread);
 
 	uif_close(ubi);
-	ubi_eba_close(ubi);
 	ubi_wl_close(ubi);
+	free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 	put_mtd_device(ubi->mtd);
 	vfree(ubi->peb_buf1);
@@ -1044,8 +1092,7 @@ static void __exit ubi_exit(void)
 module_exit(ubi_exit);
 
 /**
- * bytes_str_to_int - convert a string representing number of bytes to an
- * integer.
+ * bytes_str_to_int - convert a number of bytes string into an integer.
  * @str: the string to convert
  *
  * This function returns positive resulting integer in case of success and a
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index 89193ba9451..03c759b4eeb 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -39,9 +39,9 @@
 #include <linux/stat.h>
 #include <linux/ioctl.h>
 #include <linux/capability.h>
+#include <linux/uaccess.h>
 #include <linux/smp_lock.h>
 #include <mtd/ubi-user.h>
-#include <asm/uaccess.h>
 #include <asm/div64.h>
 #include "ubi.h"
 
@@ -116,7 +116,7 @@ static int vol_cdev_open(struct inode *inode, struct file *file)
 	else
 		mode = UBI_READONLY;
 
-	dbg_msg("open volume %d, mode %d", vol_id, mode);
+	dbg_gen("open volume %d, mode %d", vol_id, mode);
 
 	desc = ubi_open_volume(ubi_num, vol_id, mode);
 	unlock_kernel();
@@ -132,7 +132,7 @@ static int vol_cdev_release(struct inode *inode, struct file *file)
 	struct ubi_volume_desc *desc = file->private_data;
 	struct ubi_volume *vol = desc->vol;
 
-	dbg_msg("release volume %d, mode %d", vol->vol_id, desc->mode);
+	dbg_gen("release volume %d, mode %d", vol->vol_id, desc->mode);
 
 	if (vol->updating) {
 		ubi_warn("update of volume %d not finished, volume is damaged",
@@ -141,7 +141,7 @@ static int vol_cdev_release(struct inode *inode, struct file *file)
 		vol->updating = 0;
 		vfree(vol->upd_buf);
 	} else if (vol->changing_leb) {
-		dbg_msg("only %lld of %lld bytes received for atomic LEB change"
+		dbg_gen("only %lld of %lld bytes received for atomic LEB change"
 			" for volume %d:%d, cancel", vol->upd_received,
 			vol->upd_bytes, vol->ubi->ubi_num, vol->vol_id);
 		vol->changing_leb = 0;
@@ -183,7 +183,7 @@ static loff_t vol_cdev_llseek(struct file *file, loff_t offset, int origin)
 		return -EINVAL;
 	}
 
-	dbg_msg("seek volume %d, offset %lld, origin %d, new offset %lld",
+	dbg_gen("seek volume %d, offset %lld, origin %d, new offset %lld",
 		vol->vol_id, offset, origin, new_offset);
 
 	file->f_pos = new_offset;
@@ -201,7 +201,7 @@ static ssize_t vol_cdev_read(struct file *file, __user char *buf, size_t count,
 	void *tbuf;
 	uint64_t tmp;
 
-	dbg_msg("read %zd bytes from offset %lld of volume %d",
+	dbg_gen("read %zd bytes from offset %lld of volume %d",
 		count, *offp, vol->vol_id);
 
 	if (vol->updating) {
@@ -216,7 +216,7 @@ static ssize_t vol_cdev_read(struct file *file, __user char *buf, size_t count,
 		return 0;
 
 	if (vol->corrupted)
-		dbg_msg("read from corrupted volume %d", vol->vol_id);
+		dbg_gen("read from corrupted volume %d", vol->vol_id);
 
 	if (*offp + count > vol->used_bytes)
 		count_save = count = vol->used_bytes - *offp;
@@ -285,7 +285,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 	char *tbuf;
 	uint64_t tmp;
 
-	dbg_msg("requested: write %zd bytes to offset %lld of volume %u",
+	dbg_gen("requested: write %zd bytes to offset %lld of volume %u",
 		count, *offp, vol->vol_id);
 
 	if (vol->vol_type == UBI_STATIC_VOLUME)
@@ -295,7 +295,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 	off = do_div(tmp, vol->usable_leb_size);
 	lnum = tmp;
 
-	if (off % ubi->min_io_size) {
+	if (off & (ubi->min_io_size - 1)) {
 		dbg_err("unaligned position");
 		return -EINVAL;
 	}
@@ -304,7 +304,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 		count_save = count = vol->used_bytes - *offp;
 
 	/* We can write only in fractions of the minimum I/O unit */
-	if (count % ubi->min_io_size) {
+	if (count & (ubi->min_io_size - 1)) {
 		dbg_err("unaligned write length");
 		return -EINVAL;
 	}
@@ -352,7 +352,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 }
 
 #else
-#define vol_cdev_direct_write(file, buf, count, offp) -EPERM
+#define vol_cdev_direct_write(file, buf, count, offp) (-EPERM)
 #endif /* CONFIG_MTD_UBI_DEBUG_USERSPACE_IO */
 
 static ssize_t vol_cdev_write(struct file *file, const char __user *buf,
@@ -437,7 +437,8 @@ static int vol_cdev_ioctl(struct inode *inode, struct file *file,
 			break;
 		}
 
-		rsvd_bytes = vol->reserved_pebs * (ubi->leb_size-vol->data_pad);
+		rsvd_bytes = (long long)vol->reserved_pebs *
+					ubi->leb_size-vol->data_pad;
 		if (bytes < 0 || bytes > rsvd_bytes) {
 			err = -EINVAL;
 			break;
@@ -513,7 +514,7 @@ static int vol_cdev_ioctl(struct inode *inode, struct file *file,
 			break;
 		}
 
-		dbg_msg("erase LEB %d:%d", vol->vol_id, lnum);
+		dbg_gen("erase LEB %d:%d", vol->vol_id, lnum);
 		err = ubi_eba_unmap_leb(ubi, vol, lnum);
 		if (err)
 			break;
@@ -564,7 +565,7 @@ static int verify_mkvol_req(const struct ubi_device *ubi,
 	if (req->alignment > ubi->leb_size)
 		goto bad;
 
-	n = req->alignment % ubi->min_io_size;
+	n = req->alignment & (ubi->min_io_size - 1);
 	if (req->alignment != 1 && n)
 		goto bad;
 
@@ -573,6 +574,10 @@ static int verify_mkvol_req(const struct ubi_device *ubi,
 		goto bad;
 	}
 
+	n = strnlen(req->name, req->name_len + 1);
+	if (n != req->name_len)
+		goto bad;
+
 	return 0;
 
 bad:
@@ -600,6 +605,166 @@ static int verify_rsvol_req(const struct ubi_device *ubi,
 	return 0;
 }
 
+/**
+ * rename_volumes - rename UBI volumes.
+ * @ubi: UBI device description object
+ * @req: volumes re-name request
+ *
+ * This is a helper function for the volume re-name IOCTL which validates the
+ * the request, opens the volume and calls corresponding volumes management
+ * function. Returns zero in case of success and a negative error code in case
+ * of failure.
+ */
+static int rename_volumes(struct ubi_device *ubi,
+			  struct ubi_rnvol_req *req)
+{
+	int i, n, err;
+	struct list_head rename_list;
+	struct ubi_rename_entry *re, *re1;
+
+	if (req->count < 0 || req->count > UBI_MAX_RNVOL)
+		return -EINVAL;
+
+	if (req->count == 0)
+		return 0;
+
+	/* Validate volume IDs and names in the request */
+	for (i = 0; i < req->count; i++) {
+		if (req->ents[i].vol_id < 0 ||
+		    req->ents[i].vol_id >= ubi->vtbl_slots)
+			return -EINVAL;
+		if (req->ents[i].name_len < 0)
+			return -EINVAL;
+		if (req->ents[i].name_len > UBI_VOL_NAME_MAX)
+			return -ENAMETOOLONG;
+		req->ents[i].name[req->ents[i].name_len] = '\0';
+		n = strlen(req->ents[i].name);
+		if (n != req->ents[i].name_len)
+			err = -EINVAL;
+	}
+
+	/* Make sure volume IDs and names are unique */
+	for (i = 0; i < req->count - 1; i++) {
+		for (n = i + 1; n < req->count; n++) {
+			if (req->ents[i].vol_id == req->ents[n].vol_id) {
+				dbg_err("duplicated volume id %d",
+					req->ents[i].vol_id);
+				return -EINVAL;
+			}
+			if (!strcmp(req->ents[i].name, req->ents[n].name)) {
+				dbg_err("duplicated volume name \"%s\"",
+					req->ents[i].name);
+				return -EINVAL;
+			}
+		}
+	}
+
+	/* Create the re-name list */
+	INIT_LIST_HEAD(&rename_list);
+	for (i = 0; i < req->count; i++) {
+		int vol_id = req->ents[i].vol_id;
+		int name_len = req->ents[i].name_len;
+		const char *name = req->ents[i].name;
+
+		re = kzalloc(sizeof(struct ubi_rename_entry), GFP_KERNEL);
+		if (!re) {
+			err = -ENOMEM;
+			goto out_free;
+		}
+
+		re->desc = ubi_open_volume(ubi->ubi_num, vol_id, UBI_EXCLUSIVE);
+		if (IS_ERR(re->desc)) {
+			err = PTR_ERR(re->desc);
+			dbg_err("cannot open volume %d, error %d", vol_id, err);
+			kfree(re);
+			goto out_free;
+		}
+
+		/* Skip this re-naming if the name does not really change */
+		if (re->desc->vol->name_len == name_len &&
+		    !memcmp(re->desc->vol->name, name, name_len)) {
+			ubi_close_volume(re->desc);
+			kfree(re);
+			continue;
+		}
+
+		re->new_name_len = name_len;
+		memcpy(re->new_name, name, name_len);
+		list_add_tail(&re->list, &rename_list);
+		dbg_msg("will rename volume %d from \"%s\" to \"%s\"",
+			vol_id, re->desc->vol->name, name);
+	}
+
+	if (list_empty(&rename_list))
+		return 0;
+
+	/* Find out the volumes which have to be removed */
+	list_for_each_entry(re, &rename_list, list) {
+		struct ubi_volume_desc *desc;
+		int no_remove_needed = 0;
+
+		/*
+		 * Volume @re->vol_id is going to be re-named to
+		 * @re->new_name, while its current name is @name. If a volume
+		 * with name @re->new_name currently exists, it has to be
+		 * removed, unless it is also re-named in the request (@req).
+		 */
+		list_for_each_entry(re1, &rename_list, list) {
+			if (re->new_name_len == re1->desc->vol->name_len &&
+			    !memcmp(re->new_name, re1->desc->vol->name,
+				    re1->desc->vol->name_len)) {
+				no_remove_needed = 1;
+				break;
+			}
+		}
+
+		if (no_remove_needed)
+			continue;
+
+		/*
+		 * It seems we need to remove volume with name @re->new_name,
+		 * if it exists.
+		 */
+		desc = ubi_open_volume_nm(ubi->ubi_num, re->new_name, UBI_EXCLUSIVE);
+		if (IS_ERR(desc)) {
+			err = PTR_ERR(desc);
+			if (err == -ENODEV)
+				/* Re-naming into a non-existing volume name */
+				continue;
+
+			/* The volume exists but busy, or an error occurred */
+			dbg_err("cannot open volume \"%s\", error %d",
+				re->new_name, err);
+			goto out_free;
+		}
+
+		re = kzalloc(sizeof(struct ubi_rename_entry), GFP_KERNEL);
+		if (!re) {
+			err = -ENOMEM;
+			ubi_close_volume(desc);
+			goto out_free;
+		}
+
+		re->remove = 1;
+		re->desc = desc;
+		list_add(&re->list, &rename_list);
+		dbg_msg("will remove volume %d, name \"%s\"",
+			re->desc->vol->vol_id, re->desc->vol->name);
+	}
+
+	mutex_lock(&ubi->volumes_mutex);
+	err = ubi_rename_volumes(ubi, &rename_list);
+	mutex_unlock(&ubi->volumes_mutex);
+
+out_free:
+	list_for_each_entry_safe(re, re1, &rename_list, list) {
+		ubi_close_volume(re->desc);
+		list_del(&re->list);
+		kfree(re);
+	}
+	return err;
+}
+
 static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 			  unsigned int cmd, unsigned long arg)
 {
@@ -621,19 +786,18 @@ static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 	{
 		struct ubi_mkvol_req req;
 
-		dbg_msg("create volume");
+		dbg_gen("create volume");
 		err = copy_from_user(&req, argp, sizeof(struct ubi_mkvol_req));
 		if (err) {
 			err = -EFAULT;
 			break;
 		}
 
+		req.name[req.name_len] = '\0';
 		err = verify_mkvol_req(ubi, &req);
 		if (err)
 			break;
 
-		req.name[req.name_len] = '\0';
-
 		mutex_lock(&ubi->volumes_mutex);
 		err = ubi_create_volume(ubi, &req);
 		mutex_unlock(&ubi->volumes_mutex);
@@ -652,7 +816,7 @@ static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 	{
 		int vol_id;
 
-		dbg_msg("remove volume");
+		dbg_gen("remove volume");
 		err = get_user(vol_id, (__user int32_t *)argp);
 		if (err) {
 			err = -EFAULT;
@@ -666,7 +830,7 @@ static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 		}
 
 		mutex_lock(&ubi->volumes_mutex);
-		err = ubi_remove_volume(desc);
+		err = ubi_remove_volume(desc, 0);
 		mutex_unlock(&ubi->volumes_mutex);
 
 		/*
@@ -685,7 +849,7 @@ static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 		uint64_t tmp;
 		struct ubi_rsvol_req req;
 
-		dbg_msg("re-size volume");
+		dbg_gen("re-size volume");
 		err = copy_from_user(&req, argp, sizeof(struct ubi_rsvol_req));
 		if (err) {
 			err = -EFAULT;
@@ -713,6 +877,32 @@ static int ubi_cdev_ioctl(struct inode *inode, struct file *file,
 		break;
 	}
 
+	/* Re-name volumes command */
+	case UBI_IOCRNVOL:
+	{
+		struct ubi_rnvol_req *req;
+
+		dbg_msg("re-name volumes");
+		req = kmalloc(sizeof(struct ubi_rnvol_req), GFP_KERNEL);
+		if (!req) {
+			err = -ENOMEM;
+			break;
+		};
+
+		err = copy_from_user(req, argp, sizeof(struct ubi_rnvol_req));
+		if (err) {
+			err = -EFAULT;
+			kfree(req);
+			break;
+		}
+
+		mutex_lock(&ubi->mult_mutex);
+		err = rename_volumes(ubi, req);
+		mutex_unlock(&ubi->mult_mutex);
+		kfree(req);
+		break;
+	}
+
 	default:
 		err = -ENOTTY;
 		break;
@@ -738,7 +928,7 @@ static int ctrl_cdev_ioctl(struct inode *inode, struct file *file,
 		struct ubi_attach_req req;
 		struct mtd_info *mtd;
 
-		dbg_msg("attach MTD device");
+		dbg_gen("attach MTD device");
 		err = copy_from_user(&req, argp, sizeof(struct ubi_attach_req));
 		if (err) {
 			err = -EFAULT;
@@ -778,7 +968,7 @@ static int ctrl_cdev_ioctl(struct inode *inode, struct file *file,
 	{
 		int ubi_num;
 
-		dbg_msg("dettach MTD device");
+		dbg_gen("dettach MTD device");
 		err = get_user(ubi_num, (__user int32_t *)argp);
 		if (err) {
 			err = -EFAULT;
diff --git a/drivers/mtd/ubi/debug.c b/drivers/mtd/ubi/debug.c
index 56956ec2845..c0ed60e8ade 100644
--- a/drivers/mtd/ubi/debug.c
+++ b/drivers/mtd/ubi/debug.c
@@ -24,7 +24,7 @@
  * changes.
  */
 
-#ifdef CONFIG_MTD_UBI_DEBUG_MSG
+#ifdef CONFIG_MTD_UBI_DEBUG
 
 #include "ubi.h"
 
@@ -34,14 +34,19 @@
  */
 void ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)
 {
-	dbg_msg("erase counter header dump:");
-	dbg_msg("magic          %#08x", be32_to_cpu(ec_hdr->magic));
-	dbg_msg("version        %d",    (int)ec_hdr->version);
-	dbg_msg("ec             %llu",  (long long)be64_to_cpu(ec_hdr->ec));
-	dbg_msg("vid_hdr_offset %d",    be32_to_cpu(ec_hdr->vid_hdr_offset));
-	dbg_msg("data_offset    %d",    be32_to_cpu(ec_hdr->data_offset));
-	dbg_msg("hdr_crc        %#08x", be32_to_cpu(ec_hdr->hdr_crc));
-	dbg_msg("erase counter header hexdump:");
+	printk(KERN_DEBUG "Erase counter header dump:\n");
+	printk(KERN_DEBUG "\tmagic          %#08x\n",
+	       be32_to_cpu(ec_hdr->magic));
+	printk(KERN_DEBUG "\tversion        %d\n", (int)ec_hdr->version);
+	printk(KERN_DEBUG "\tec             %llu\n",
+	       (long long)be64_to_cpu(ec_hdr->ec));
+	printk(KERN_DEBUG "\tvid_hdr_offset %d\n",
+	       be32_to_cpu(ec_hdr->vid_hdr_offset));
+	printk(KERN_DEBUG "\tdata_offset    %d\n",
+	       be32_to_cpu(ec_hdr->data_offset));
+	printk(KERN_DEBUG "\thdr_crc        %#08x\n",
+	       be32_to_cpu(ec_hdr->hdr_crc));
+	printk(KERN_DEBUG "erase counter header hexdump:\n");
 	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
 		       ec_hdr, UBI_EC_HDR_SIZE, 1);
 }
@@ -52,22 +57,23 @@ void ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)
  */
 void ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)
 {
-	dbg_msg("volume identifier header dump:");
-	dbg_msg("magic     %08x", be32_to_cpu(vid_hdr->magic));
-	dbg_msg("version   %d",   (int)vid_hdr->version);
-	dbg_msg("vol_type  %d",   (int)vid_hdr->vol_type);
-	dbg_msg("copy_flag %d",   (int)vid_hdr->copy_flag);
-	dbg_msg("compat    %d",   (int)vid_hdr->compat);
-	dbg_msg("vol_id    %d",   be32_to_cpu(vid_hdr->vol_id));
-	dbg_msg("lnum      %d",   be32_to_cpu(vid_hdr->lnum));
-	dbg_msg("leb_ver   %u",   be32_to_cpu(vid_hdr->leb_ver));
-	dbg_msg("data_size %d",   be32_to_cpu(vid_hdr->data_size));
-	dbg_msg("used_ebs  %d",   be32_to_cpu(vid_hdr->used_ebs));
-	dbg_msg("data_pad  %d",   be32_to_cpu(vid_hdr->data_pad));
-	dbg_msg("sqnum     %llu",
+	printk(KERN_DEBUG "Volume identifier header dump:\n");
+	printk(KERN_DEBUG "\tmagic     %08x\n", be32_to_cpu(vid_hdr->magic));
+	printk(KERN_DEBUG "\tversion   %d\n",   (int)vid_hdr->version);
+	printk(KERN_DEBUG "\tvol_type  %d\n",   (int)vid_hdr->vol_type);
+	printk(KERN_DEBUG "\tcopy_flag %d\n",   (int)vid_hdr->copy_flag);
+	printk(KERN_DEBUG "\tcompat    %d\n",   (int)vid_hdr->compat);
+	printk(KERN_DEBUG "\tvol_id    %d\n",   be32_to_cpu(vid_hdr->vol_id));
+	printk(KERN_DEBUG "\tlnum      %d\n",   be32_to_cpu(vid_hdr->lnum));
+	printk(KERN_DEBUG "\tdata_size %d\n",   be32_to_cpu(vid_hdr->data_size));
+	printk(KERN_DEBUG "\tused_ebs  %d\n",   be32_to_cpu(vid_hdr->used_ebs));
+	printk(KERN_DEBUG "\tdata_pad  %d\n",   be32_to_cpu(vid_hdr->data_pad));
+	printk(KERN_DEBUG "\tsqnum     %llu\n",
 		(unsigned long long)be64_to_cpu(vid_hdr->sqnum));
-	dbg_msg("hdr_crc   %08x", be32_to_cpu(vid_hdr->hdr_crc));
-	dbg_msg("volume identifier header hexdump:");
+	printk(KERN_DEBUG "\thdr_crc   %08x\n", be32_to_cpu(vid_hdr->hdr_crc));
+	printk(KERN_DEBUG "Volume identifier header hexdump:\n");
+	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
+		       vid_hdr, UBI_VID_HDR_SIZE, 1);
 }
 
 /**
@@ -76,27 +82,27 @@ void ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)
  */
 void ubi_dbg_dump_vol_info(const struct ubi_volume *vol)
 {
-	dbg_msg("volume information dump:");
-	dbg_msg("vol_id          %d", vol->vol_id);
-	dbg_msg("reserved_pebs   %d", vol->reserved_pebs);
-	dbg_msg("alignment       %d", vol->alignment);
-	dbg_msg("data_pad        %d", vol->data_pad);
-	dbg_msg("vol_type        %d", vol->vol_type);
-	dbg_msg("name_len        %d", vol->name_len);
-	dbg_msg("usable_leb_size %d", vol->usable_leb_size);
-	dbg_msg("used_ebs        %d", vol->used_ebs);
-	dbg_msg("used_bytes      %lld", vol->used_bytes);
-	dbg_msg("last_eb_bytes   %d", vol->last_eb_bytes);
-	dbg_msg("corrupted       %d", vol->corrupted);
-	dbg_msg("upd_marker      %d", vol->upd_marker);
+	printk(KERN_DEBUG "Volume information dump:\n");
+	printk(KERN_DEBUG "\tvol_id          %d\n", vol->vol_id);
+	printk(KERN_DEBUG "\treserved_pebs   %d\n", vol->reserved_pebs);
+	printk(KERN_DEBUG "\talignment       %d\n", vol->alignment);
+	printk(KERN_DEBUG "\tdata_pad        %d\n", vol->data_pad);
+	printk(KERN_DEBUG "\tvol_type        %d\n", vol->vol_type);
+	printk(KERN_DEBUG "\tname_len        %d\n", vol->name_len);
+	printk(KERN_DEBUG "\tusable_leb_size %d\n", vol->usable_leb_size);
+	printk(KERN_DEBUG "\tused_ebs        %d\n", vol->used_ebs);
+	printk(KERN_DEBUG "\tused_bytes      %lld\n", vol->used_bytes);
+	printk(KERN_DEBUG "\tlast_eb_bytes   %d\n", vol->last_eb_bytes);
+	printk(KERN_DEBUG "\tcorrupted       %d\n", vol->corrupted);
+	printk(KERN_DEBUG "\tupd_marker      %d\n", vol->upd_marker);
 
 	if (vol->name_len <= UBI_VOL_NAME_MAX &&
 	    strnlen(vol->name, vol->name_len + 1) == vol->name_len) {
-		dbg_msg("name            %s", vol->name);
+		printk(KERN_DEBUG "\tname            %s\n", vol->name);
 	} else {
-		dbg_msg("the 1st 5 characters of the name: %c%c%c%c%c",
-			vol->name[0], vol->name[1], vol->name[2],
-			vol->name[3], vol->name[4]);
+		printk(KERN_DEBUG "\t1st 5 characters of name: %c%c%c%c%c\n",
+		       vol->name[0], vol->name[1], vol->name[2],
+		       vol->name[3], vol->name[4]);
 	}
 }
 
@@ -109,28 +115,29 @@ void ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)
 {
 	int name_len = be16_to_cpu(r->name_len);
 
-	dbg_msg("volume table record %d dump:", idx);
-	dbg_msg("reserved_pebs   %d", be32_to_cpu(r->reserved_pebs));
-	dbg_msg("alignment       %d", be32_to_cpu(r->alignment));
-	dbg_msg("data_pad        %d", be32_to_cpu(r->data_pad));
-	dbg_msg("vol_type        %d", (int)r->vol_type);
-	dbg_msg("upd_marker      %d", (int)r->upd_marker);
-	dbg_msg("name_len        %d", name_len);
+	printk(KERN_DEBUG "Volume table record %d dump:\n", idx);
+	printk(KERN_DEBUG "\treserved_pebs   %d\n",
+	       be32_to_cpu(r->reserved_pebs));
+	printk(KERN_DEBUG "\talignment       %d\n", be32_to_cpu(r->alignment));
+	printk(KERN_DEBUG "\tdata_pad        %d\n", be32_to_cpu(r->data_pad));
+	printk(KERN_DEBUG "\tvol_type        %d\n", (int)r->vol_type);
+	printk(KERN_DEBUG "\tupd_marker      %d\n", (int)r->upd_marker);
+	printk(KERN_DEBUG "\tname_len        %d\n", name_len);
 
 	if (r->name[0] == '\0') {
-		dbg_msg("name            NULL");
+		printk(KERN_DEBUG "\tname            NULL\n");
 		return;
 	}
 
 	if (name_len <= UBI_VOL_NAME_MAX &&
 	    strnlen(&r->name[0], name_len + 1) == name_len) {
-		dbg_msg("name            %s", &r->name[0]);
+		printk(KERN_DEBUG "\tname            %s\n", &r->name[0]);
 	} else {
-		dbg_msg("1st 5 characters of the name: %c%c%c%c%c",
+		printk(KERN_DEBUG "\t1st 5 characters of name: %c%c%c%c%c\n",
 			r->name[0], r->name[1], r->name[2], r->name[3],
 			r->name[4]);
 	}
-	dbg_msg("crc             %#08x", be32_to_cpu(r->crc));
+	printk(KERN_DEBUG "\tcrc             %#08x\n", be32_to_cpu(r->crc));
 }
 
 /**
@@ -139,15 +146,15 @@ void ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)
  */
 void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv)
 {
-	dbg_msg("volume scanning information dump:");
-	dbg_msg("vol_id         %d", sv->vol_id);
-	dbg_msg("highest_lnum   %d", sv->highest_lnum);
-	dbg_msg("leb_count      %d", sv->leb_count);
-	dbg_msg("compat         %d", sv->compat);
-	dbg_msg("vol_type       %d", sv->vol_type);
-	dbg_msg("used_ebs       %d", sv->used_ebs);
-	dbg_msg("last_data_size %d", sv->last_data_size);
-	dbg_msg("data_pad       %d", sv->data_pad);
+	printk(KERN_DEBUG "Volume scanning information dump:\n");
+	printk(KERN_DEBUG "\tvol_id         %d\n", sv->vol_id);
+	printk(KERN_DEBUG "\thighest_lnum   %d\n", sv->highest_lnum);
+	printk(KERN_DEBUG "\tleb_count      %d\n", sv->leb_count);
+	printk(KERN_DEBUG "\tcompat         %d\n", sv->compat);
+	printk(KERN_DEBUG "\tvol_type       %d\n", sv->vol_type);
+	printk(KERN_DEBUG "\tused_ebs       %d\n", sv->used_ebs);
+	printk(KERN_DEBUG "\tlast_data_size %d\n", sv->last_data_size);
+	printk(KERN_DEBUG "\tdata_pad       %d\n", sv->data_pad);
 }
 
 /**
@@ -157,14 +164,13 @@ void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv)
  */
 void ubi_dbg_dump_seb(const struct ubi_scan_leb *seb, int type)
 {
-	dbg_msg("eraseblock scanning information dump:");
-	dbg_msg("ec       %d", seb->ec);
-	dbg_msg("pnum     %d", seb->pnum);
+	printk(KERN_DEBUG "eraseblock scanning information dump:\n");
+	printk(KERN_DEBUG "\tec       %d\n", seb->ec);
+	printk(KERN_DEBUG "\tpnum     %d\n", seb->pnum);
 	if (type == 0) {
-		dbg_msg("lnum     %d", seb->lnum);
-		dbg_msg("scrub    %d", seb->scrub);
-		dbg_msg("sqnum    %llu", seb->sqnum);
-		dbg_msg("leb_ver  %u", seb->leb_ver);
+		printk(KERN_DEBUG "\tlnum     %d\n", seb->lnum);
+		printk(KERN_DEBUG "\tscrub    %d\n", seb->scrub);
+		printk(KERN_DEBUG "\tsqnum    %llu\n", seb->sqnum);
 	}
 }
 
@@ -176,16 +182,16 @@ void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req)
 {
 	char nm[17];
 
-	dbg_msg("volume creation request dump:");
-	dbg_msg("vol_id    %d",   req->vol_id);
-	dbg_msg("alignment %d",   req->alignment);
-	dbg_msg("bytes     %lld", (long long)req->bytes);
-	dbg_msg("vol_type  %d",   req->vol_type);
-	dbg_msg("name_len  %d",   req->name_len);
+	printk(KERN_DEBUG "Volume creation request dump:\n");
+	printk(KERN_DEBUG "\tvol_id    %d\n",   req->vol_id);
+	printk(KERN_DEBUG "\talignment %d\n",   req->alignment);
+	printk(KERN_DEBUG "\tbytes     %lld\n", (long long)req->bytes);
+	printk(KERN_DEBUG "\tvol_type  %d\n",   req->vol_type);
+	printk(KERN_DEBUG "\tname_len  %d\n",   req->name_len);
 
 	memcpy(nm, req->name, 16);
 	nm[16] = 0;
-	dbg_msg("the 1st 16 characters of the name: %s", nm);
+	printk(KERN_DEBUG "\t1st 16 characters of name: %s\n", nm);
 }
 
-#endif /* CONFIG_MTD_UBI_DEBUG_MSG */
+#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/mtd/ubi/debug.h b/drivers/mtd/ubi/debug.h
index 8ea99d8c9e1..78e914d23ec 100644
--- a/drivers/mtd/ubi/debug.h
+++ b/drivers/mtd/ubi/debug.h
@@ -24,21 +24,16 @@
 #ifdef CONFIG_MTD_UBI_DEBUG
 #include <linux/random.h>
 
-#define ubi_assert(expr)  BUG_ON(!(expr))
 #define dbg_err(fmt, ...) ubi_err(fmt, ##__VA_ARGS__)
-#else
-#define ubi_assert(expr)  ({})
-#define dbg_err(fmt, ...) ({})
-#endif
 
-#ifdef CONFIG_MTD_UBI_DEBUG_DISABLE_BGT
-#define DBG_DISABLE_BGT 1
-#else
-#define DBG_DISABLE_BGT 0
-#endif
+#define ubi_assert(expr)  do {                                               \
+        if (unlikely(!(expr))) {                                             \
+                printk(KERN_CRIT "UBI assert failed in %s at %u (pid %d)\n", \
+                       __func__, __LINE__, current->pid);                    \
+                ubi_dbg_dump_stack();                                        \
+        }                                                                    \
+} while (0)
 
-#ifdef CONFIG_MTD_UBI_DEBUG_MSG
-/* Generic debugging message */
 #define dbg_msg(fmt, ...)                                    \
 	printk(KERN_DEBUG "UBI DBG (pid %d): %s: " fmt "\n", \
 	       current->pid, __func__, ##__VA_ARGS__)
@@ -61,36 +56,29 @@ void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv);
 void ubi_dbg_dump_seb(const struct ubi_scan_leb *seb, int type);
 void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req);
 
+#ifdef CONFIG_MTD_UBI_DEBUG_MSG
+/* General debugging messages */
+#define dbg_gen(fmt, ...) dbg_msg(fmt, ##__VA_ARGS__)
 #else
-
-#define dbg_msg(fmt, ...)    ({})
-#define ubi_dbg_dump_stack() ({})
-#define ubi_dbg_dump_ec_hdr(ec_hdr)      ({})
-#define ubi_dbg_dump_vid_hdr(vid_hdr)    ({})
-#define ubi_dbg_dump_vol_info(vol)       ({})
-#define ubi_dbg_dump_vtbl_record(r, idx) ({})
-#define ubi_dbg_dump_sv(sv)              ({})
-#define ubi_dbg_dump_seb(seb, type)      ({})
-#define ubi_dbg_dump_mkvol_req(req)      ({})
-
-#endif /* CONFIG_MTD_UBI_DEBUG_MSG */
+#define dbg_gen(fmt, ...) ({})
+#endif
 
 #ifdef CONFIG_MTD_UBI_DEBUG_MSG_EBA
-/* Messages from the eraseblock association unit */
+/* Messages from the eraseblock association sub-system */
 #define dbg_eba(fmt, ...) dbg_msg(fmt, ##__VA_ARGS__)
 #else
 #define dbg_eba(fmt, ...) ({})
 #endif
 
 #ifdef CONFIG_MTD_UBI_DEBUG_MSG_WL
-/* Messages from the wear-leveling unit */
+/* Messages from the wear-leveling sub-system */
 #define dbg_wl(fmt, ...) dbg_msg(fmt, ##__VA_ARGS__)
 #else
 #define dbg_wl(fmt, ...) ({})
 #endif
 
 #ifdef CONFIG_MTD_UBI_DEBUG_MSG_IO
-/* Messages from the input/output unit */
+/* Messages from the input/output sub-system */
 #define dbg_io(fmt, ...) dbg_msg(fmt, ##__VA_ARGS__)
 #else
 #define dbg_io(fmt, ...) ({})
@@ -105,6 +93,12 @@ void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req);
 #define UBI_IO_DEBUG 0
 #endif
 
+#ifdef CONFIG_MTD_UBI_DEBUG_DISABLE_BGT
+#define DBG_DISABLE_BGT 1
+#else
+#define DBG_DISABLE_BGT 0
+#endif
+
 #ifdef CONFIG_MTD_UBI_DEBUG_EMULATE_BITFLIPS
 /**
  * ubi_dbg_is_bitflip - if it is time to emulate a bit-flip.
@@ -149,4 +143,30 @@ static inline int ubi_dbg_is_erase_failure(void)
 #define ubi_dbg_is_erase_failure() 0
 #endif
 
+#else
+
+#define ubi_assert(expr)                 ({})
+#define dbg_err(fmt, ...)                ({})
+#define dbg_msg(fmt, ...)                ({})
+#define dbg_gen(fmt, ...)                ({})
+#define dbg_eba(fmt, ...)                ({})
+#define dbg_wl(fmt, ...)                 ({})
+#define dbg_io(fmt, ...)                 ({})
+#define dbg_bld(fmt, ...)                ({})
+#define ubi_dbg_dump_stack()             ({})
+#define ubi_dbg_dump_ec_hdr(ec_hdr)      ({})
+#define ubi_dbg_dump_vid_hdr(vid_hdr)    ({})
+#define ubi_dbg_dump_vol_info(vol)       ({})
+#define ubi_dbg_dump_vtbl_record(r, idx) ({})
+#define ubi_dbg_dump_sv(sv)              ({})
+#define ubi_dbg_dump_seb(seb, type)      ({})
+#define ubi_dbg_dump_mkvol_req(req)      ({})
+
+#define UBI_IO_DEBUG               0
+#define DBG_DISABLE_BGT            0
+#define ubi_dbg_is_bitflip()       0
+#define ubi_dbg_is_write_failure() 0
+#define ubi_dbg_is_erase_failure() 0
+
+#endif /* !CONFIG_MTD_UBI_DEBUG */
 #endif /* !__UBI_DEBUG_H__ */
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index 7ce91ca742b..e04bcf1dff8 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -19,20 +19,20 @@
  */
 
 /*
- * The UBI Eraseblock Association (EBA) unit.
+ * The UBI Eraseblock Association (EBA) sub-system.
  *
- * This unit is responsible for I/O to/from logical eraseblock.
+ * This sub-system is responsible for I/O to/from logical eraseblock.
  *
  * Although in this implementation the EBA table is fully kept and managed in
  * RAM, which assumes poor scalability, it might be (partially) maintained on
  * flash in future implementations.
  *
- * The EBA unit implements per-logical eraseblock locking. Before accessing a
- * logical eraseblock it is locked for reading or writing. The per-logical
- * eraseblock locking is implemented by means of the lock tree. The lock tree
- * is an RB-tree which refers all the currently locked logical eraseblocks. The
- * lock tree elements are &struct ubi_ltree_entry objects. They are indexed by
- * (@vol_id, @lnum) pairs.
+ * The EBA sub-system implements per-logical eraseblock locking. Before
+ * accessing a logical eraseblock it is locked for reading or writing. The
+ * per-logical eraseblock locking is implemented by means of the lock tree. The
+ * lock tree is an RB-tree which refers all the currently locked logical
+ * eraseblocks. The lock tree elements are &struct ubi_ltree_entry objects.
+ * They are indexed by (@vol_id, @lnum) pairs.
  *
  * EBA also maintains the global sequence counter which is incremented each
  * time a logical eraseblock is mapped to a physical eraseblock and it is
@@ -189,9 +189,7 @@ static struct ubi_ltree_entry *ltree_add_entry(struct ubi_device *ubi,
 	le->users += 1;
 	spin_unlock(&ubi->ltree_lock);
 
-	if (le_free)
-		kfree(le_free);
-
+	kfree(le_free);
 	return le;
 }
 
@@ -223,22 +221,18 @@ static int leb_read_lock(struct ubi_device *ubi, int vol_id, int lnum)
  */
 static void leb_read_unlock(struct ubi_device *ubi, int vol_id, int lnum)
 {
-	int free = 0;
 	struct ubi_ltree_entry *le;
 
 	spin_lock(&ubi->ltree_lock);
 	le = ltree_lookup(ubi, vol_id, lnum);
 	le->users -= 1;
 	ubi_assert(le->users >= 0);
+	up_read(&le->mutex);
 	if (le->users == 0) {
 		rb_erase(&le->rb, &ubi->ltree);
-		free = 1;
+		kfree(le);
 	}
 	spin_unlock(&ubi->ltree_lock);
-
-	up_read(&le->mutex);
-	if (free)
-		kfree(le);
 }
 
 /**
@@ -274,7 +268,6 @@ static int leb_write_lock(struct ubi_device *ubi, int vol_id, int lnum)
  */
 static int leb_write_trylock(struct ubi_device *ubi, int vol_id, int lnum)
 {
-	int free;
 	struct ubi_ltree_entry *le;
 
 	le = ltree_add_entry(ubi, vol_id, lnum);
@@ -289,12 +282,9 @@ static int leb_write_trylock(struct ubi_device *ubi, int vol_id, int lnum)
 	ubi_assert(le->users >= 0);
 	if (le->users == 0) {
 		rb_erase(&le->rb, &ubi->ltree);
-		free = 1;
-	} else
-		free = 0;
-	spin_unlock(&ubi->ltree_lock);
-	if (free)
 		kfree(le);
+	}
+	spin_unlock(&ubi->ltree_lock);
 
 	return 1;
 }
@@ -307,23 +297,18 @@ static int leb_write_trylock(struct ubi_device *ubi, int vol_id, int lnum)
  */
 static void leb_write_unlock(struct ubi_device *ubi, int vol_id, int lnum)
 {
-	int free;
 	struct ubi_ltree_entry *le;
 
 	spin_lock(&ubi->ltree_lock);
 	le = ltree_lookup(ubi, vol_id, lnum);
 	le->users -= 1;
 	ubi_assert(le->users >= 0);
+	up_write(&le->mutex);
 	if (le->users == 0) {
 		rb_erase(&le->rb, &ubi->ltree);
-		free = 1;
-	} else
-		free = 0;
-	spin_unlock(&ubi->ltree_lock);
-
-	up_write(&le->mutex);
-	if (free)
 		kfree(le);
+	}
+	spin_unlock(&ubi->ltree_lock);
 }
 
 /**
@@ -516,9 +501,8 @@ static int recover_peb(struct ubi_device *ubi, int pnum, int vol_id, int lnum,
 	struct ubi_vid_hdr *vid_hdr;
 
 	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr) {
+	if (!vid_hdr)
 		return -ENOMEM;
-	}
 
 	mutex_lock(&ubi->buf_mutex);
 
@@ -752,7 +736,7 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 		/* If this is the last LEB @len may be unaligned */
 		len = ALIGN(data_size, ubi->min_io_size);
 	else
-		ubi_assert(len % ubi->min_io_size == 0);
+		ubi_assert(!(len & (ubi->min_io_size - 1)));
 
 	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
 	if (!vid_hdr)
@@ -919,7 +903,7 @@ retry:
 	}
 
 	if (vol->eba_tbl[lnum] >= 0) {
-		err = ubi_wl_put_peb(ubi, vol->eba_tbl[lnum], 1);
+		err = ubi_wl_put_peb(ubi, vol->eba_tbl[lnum], 0);
 		if (err)
 			goto out_leb_unlock;
 	}
@@ -1141,7 +1125,7 @@ out_unlock_leb:
 }
 
 /**
- * ubi_eba_init_scan - initialize the EBA unit using scanning information.
+ * ubi_eba_init_scan - initialize the EBA sub-system using scanning information.
  * @ubi: UBI device description object
  * @si: scanning information
  *
@@ -1156,7 +1140,7 @@ int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 	struct ubi_scan_leb *seb;
 	struct rb_node *rb;
 
-	dbg_eba("initialize EBA unit");
+	dbg_eba("initialize EBA sub-system");
 
 	spin_lock_init(&ubi->ltree_lock);
 	mutex_init(&ubi->alc_mutex);
@@ -1222,7 +1206,7 @@ int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 		ubi->rsvd_pebs  += ubi->beb_rsvd_pebs;
 	}
 
-	dbg_eba("EBA unit is initialized");
+	dbg_eba("EBA sub-system is initialized");
 	return 0;
 
 out_free:
@@ -1233,20 +1217,3 @@ out_free:
 	}
 	return err;
 }
-
-/**
- * ubi_eba_close - close EBA unit.
- * @ubi: UBI device description object
- */
-void ubi_eba_close(const struct ubi_device *ubi)
-{
-	int i, num_volumes = ubi->vtbl_slots + UBI_INT_VOL_COUNT;
-
-	dbg_eba("close EBA unit");
-
-	for (i = 0; i < num_volumes; i++) {
-		if (!ubi->volumes[i])
-			continue;
-		kfree(ubi->volumes[i]->eba_tbl);
-	}
-}
diff --git a/drivers/mtd/ubi/gluebi.c b/drivers/mtd/ubi/gluebi.c
index e909b390069..605812bb0b1 100644
--- a/drivers/mtd/ubi/gluebi.c
+++ b/drivers/mtd/ubi/gluebi.c
@@ -111,7 +111,7 @@ static int gluebi_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct ubi_device *ubi;
 	uint64_t tmp = from;
 
-	dbg_msg("read %zd bytes from offset %lld", len, from);
+	dbg_gen("read %zd bytes from offset %lld", len, from);
 
 	if (len < 0 || from < 0 || from + len > mtd->size)
 		return -EINVAL;
@@ -162,7 +162,7 @@ static int gluebi_write(struct mtd_info *mtd, loff_t to, size_t len,
 	struct ubi_device *ubi;
 	uint64_t tmp = to;
 
-	dbg_msg("write %zd bytes to offset %lld", len, to);
+	dbg_gen("write %zd bytes to offset %lld", len, to);
 
 	if (len < 0 || to < 0 || len + to > mtd->size)
 		return -EINVAL;
@@ -215,7 +215,7 @@ static int gluebi_erase(struct mtd_info *mtd, struct erase_info *instr)
 	struct ubi_volume *vol;
 	struct ubi_device *ubi;
 
-	dbg_msg("erase %u bytes at offset %u", instr->len, instr->addr);
+	dbg_gen("erase %u bytes at offset %u", instr->len, instr->addr);
 
 	if (instr->addr < 0 || instr->addr > mtd->size - mtd->erasesize)
 		return -EINVAL;
@@ -249,8 +249,8 @@ static int gluebi_erase(struct mtd_info *mtd, struct erase_info *instr)
 	if (err)
 		goto out_err;
 
-        instr->state = MTD_ERASE_DONE;
-        mtd_erase_callback(instr);
+	instr->state = MTD_ERASE_DONE;
+	mtd_erase_callback(instr);
 	return 0;
 
 out_err:
@@ -299,12 +299,12 @@ int ubi_create_gluebi(struct ubi_device *ubi, struct ubi_volume *vol)
 		mtd->size = vol->used_bytes;
 
 	if (add_mtd_device(mtd)) {
-		ubi_err("cannot not add MTD device\n");
+		ubi_err("cannot not add MTD device");
 		kfree(mtd->name);
 		return -ENFILE;
 	}
 
-	dbg_msg("added mtd%d (\"%s\"), size %u, EB size %u",
+	dbg_gen("added mtd%d (\"%s\"), size %u, EB size %u",
 		mtd->index, mtd->name, mtd->size, mtd->erasesize);
 	return 0;
 }
@@ -322,7 +322,7 @@ int ubi_destroy_gluebi(struct ubi_volume *vol)
 	int err;
 	struct mtd_info *mtd = &vol->gluebi_mtd;
 
-	dbg_msg("remove mtd%d", mtd->index);
+	dbg_gen("remove mtd%d", mtd->index);
 	err = del_mtd_device(mtd);
 	if (err)
 		return err;
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index 4ac11df7b04..2fb64be44f1 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -20,15 +20,15 @@
  */
 
 /*
- * UBI input/output unit.
+ * UBI input/output sub-system.
  *
- * This unit provides a uniform way to work with all kinds of the underlying
- * MTD devices. It also implements handy functions for reading and writing UBI
- * headers.
+ * This sub-system provides a uniform way to work with all kinds of the
+ * underlying MTD devices. It also implements handy functions for reading and
+ * writing UBI headers.
  *
  * We are trying to have a paranoid mindset and not to trust to what we read
- * from the flash media in order to be more secure and robust. So this unit
- * validates every single header it reads from the flash media.
+ * from the flash media in order to be more secure and robust. So this
+ * sub-system validates every single header it reads from the flash media.
  *
  * Some words about how the eraseblock headers are stored.
  *
@@ -79,11 +79,11 @@
  * 512-byte chunks, we have to allocate one more buffer and copy our VID header
  * to offset 448 of this buffer.
  *
- * The I/O unit does the following trick in order to avoid this extra copy.
- * It always allocates a @ubi->vid_hdr_alsize bytes buffer for the VID header
- * and returns a pointer to offset @ubi->vid_hdr_shift of this buffer. When the
- * VID header is being written out, it shifts the VID header pointer back and
- * writes the whole sub-page.
+ * The I/O sub-system does the following trick in order to avoid this extra
+ * copy. It always allocates a @ubi->vid_hdr_alsize bytes buffer for the VID
+ * header and returns a pointer to offset @ubi->vid_hdr_shift of this buffer.
+ * When the VID header is being written out, it shifts the VID header pointer
+ * back and writes the whole sub-page.
  */
 
 #include <linux/crc32.h>
@@ -156,15 +156,19 @@ retry:
 			/*
 			 * -EUCLEAN is reported if there was a bit-flip which
 			 * was corrected, so this is harmless.
+			 *
+			 * We do not report about it here unless debugging is
+			 * enabled. A corresponding message will be printed
+			 * later, when it is has been scrubbed.
 			 */
-			ubi_msg("fixable bit-flip detected at PEB %d", pnum);
+			dbg_msg("fixable bit-flip detected at PEB %d", pnum);
 			ubi_assert(len == read);
 			return UBI_IO_BITFLIPS;
 		}
 
 		if (read != len && retries++ < UBI_IO_RETRIES) {
-			dbg_io("error %d while reading %d bytes from PEB %d:%d, "
-			       "read only %zd bytes, retry",
+			dbg_io("error %d while reading %d bytes from PEB %d:%d,"
+			       " read only %zd bytes, retry",
 			       err, len, pnum, offset, read);
 			yield();
 			goto retry;
@@ -187,7 +191,7 @@ retry:
 		ubi_assert(len == read);
 
 		if (ubi_dbg_is_bitflip()) {
-			dbg_msg("bit-flip (emulated)");
+			dbg_gen("bit-flip (emulated)");
 			err = UBI_IO_BITFLIPS;
 		}
 	}
@@ -391,6 +395,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 {
 	int err, i, patt_count;
 
+	ubi_msg("run torture test for PEB %d", pnum);
 	patt_count = ARRAY_SIZE(patterns);
 	ubi_assert(patt_count > 0);
 
@@ -434,6 +439,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 	}
 
 	err = patt_count;
+	ubi_msg("PEB %d passed torture test, do not mark it a bad", pnum);
 
 out:
 	mutex_unlock(&ubi->buf_mutex);
@@ -699,8 +705,8 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 
 	if (hdr_crc != crc) {
 		if (verbose) {
-			ubi_warn("bad EC header CRC at PEB %d, calculated %#08x,"
-				 " read %#08x", pnum, crc, hdr_crc);
+			ubi_warn("bad EC header CRC at PEB %d, calculated "
+				 "%#08x, read %#08x", pnum, crc, hdr_crc);
 			ubi_dbg_dump_ec_hdr(ec_hdr);
 		}
 		return UBI_IO_BAD_EC_HDR;
@@ -1095,8 +1101,7 @@ fail:
 }
 
 /**
- * paranoid_check_peb_ec_hdr - check that the erase counter header of a
- * physical eraseblock is in-place and is all right.
+ * paranoid_check_peb_ec_hdr - check erase counter header.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  *
@@ -1174,8 +1179,7 @@ fail:
 }
 
 /**
- * paranoid_check_peb_vid_hdr - check that the volume identifier header of a
- * physical eraseblock is in-place and is all right.
+ * paranoid_check_peb_vid_hdr - check volume identifier header.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  *
@@ -1256,7 +1260,7 @@ static int paranoid_check_all_ff(struct ubi_device *ubi, int pnum, int offset,
 
 fail:
 	ubi_err("paranoid check failed for PEB %d", pnum);
-	dbg_msg("hex dump of the %d-%d region", offset, offset + len);
+	ubi_msg("hex dump of the %d-%d region", offset, offset + len);
 	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
 		       ubi->dbg_peb_buf, len, 1);
 	err = 1;
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index a70d58823f8..5d9bcf109c1 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -106,7 +106,7 @@ struct ubi_volume_desc *ubi_open_volume(int ubi_num, int vol_id, int mode)
 	struct ubi_device *ubi;
 	struct ubi_volume *vol;
 
-	dbg_msg("open device %d volume %d, mode %d", ubi_num, vol_id, mode);
+	dbg_gen("open device %d volume %d, mode %d", ubi_num, vol_id, mode);
 
 	if (ubi_num < 0 || ubi_num >= UBI_MAX_DEVICES)
 		return ERR_PTR(-EINVAL);
@@ -215,7 +215,7 @@ struct ubi_volume_desc *ubi_open_volume_nm(int ubi_num, const char *name,
 	struct ubi_device *ubi;
 	struct ubi_volume_desc *ret;
 
-	dbg_msg("open volume %s, mode %d", name, mode);
+	dbg_gen("open volume %s, mode %d", name, mode);
 
 	if (!name)
 		return ERR_PTR(-EINVAL);
@@ -266,7 +266,7 @@ void ubi_close_volume(struct ubi_volume_desc *desc)
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
 
-	dbg_msg("close volume %d, mode %d", vol->vol_id, desc->mode);
+	dbg_gen("close volume %d, mode %d", vol->vol_id, desc->mode);
 
 	spin_lock(&ubi->volumes_lock);
 	switch (desc->mode) {
@@ -323,7 +323,7 @@ int ubi_leb_read(struct ubi_volume_desc *desc, int lnum, char *buf, int offset,
 	struct ubi_device *ubi = vol->ubi;
 	int err, vol_id = vol->vol_id;
 
-	dbg_msg("read %d bytes from LEB %d:%d:%d", len, vol_id, lnum, offset);
+	dbg_gen("read %d bytes from LEB %d:%d:%d", len, vol_id, lnum, offset);
 
 	if (vol_id < 0 || vol_id >= ubi->vtbl_slots || lnum < 0 ||
 	    lnum >= vol->used_ebs || offset < 0 || len < 0 ||
@@ -388,7 +388,7 @@ int ubi_leb_write(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	struct ubi_device *ubi = vol->ubi;
 	int vol_id = vol->vol_id;
 
-	dbg_msg("write %d bytes to LEB %d:%d:%d", len, vol_id, lnum, offset);
+	dbg_gen("write %d bytes to LEB %d:%d:%d", len, vol_id, lnum, offset);
 
 	if (vol_id < 0 || vol_id >= ubi->vtbl_slots)
 		return -EINVAL;
@@ -397,8 +397,8 @@ int ubi_leb_write(struct ubi_volume_desc *desc, int lnum, const void *buf,
 		return -EROFS;
 
 	if (lnum < 0 || lnum >= vol->reserved_pebs || offset < 0 || len < 0 ||
-	    offset + len > vol->usable_leb_size || offset % ubi->min_io_size ||
-	    len % ubi->min_io_size)
+	    offset + len > vol->usable_leb_size ||
+	    offset & (ubi->min_io_size - 1) || len & (ubi->min_io_size - 1))
 		return -EINVAL;
 
 	if (dtype != UBI_LONGTERM && dtype != UBI_SHORTTERM &&
@@ -438,7 +438,7 @@ int ubi_leb_change(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	struct ubi_device *ubi = vol->ubi;
 	int vol_id = vol->vol_id;
 
-	dbg_msg("atomically write %d bytes to LEB %d:%d", len, vol_id, lnum);
+	dbg_gen("atomically write %d bytes to LEB %d:%d", len, vol_id, lnum);
 
 	if (vol_id < 0 || vol_id >= ubi->vtbl_slots)
 		return -EINVAL;
@@ -447,7 +447,7 @@ int ubi_leb_change(struct ubi_volume_desc *desc, int lnum, const void *buf,
 		return -EROFS;
 
 	if (lnum < 0 || lnum >= vol->reserved_pebs || len < 0 ||
-	    len > vol->usable_leb_size || len % ubi->min_io_size)
+	    len > vol->usable_leb_size || len & (ubi->min_io_size - 1))
 		return -EINVAL;
 
 	if (dtype != UBI_LONGTERM && dtype != UBI_SHORTTERM &&
@@ -482,7 +482,7 @@ int ubi_leb_erase(struct ubi_volume_desc *desc, int lnum)
 	struct ubi_device *ubi = vol->ubi;
 	int err;
 
-	dbg_msg("erase LEB %d:%d", vol->vol_id, lnum);
+	dbg_gen("erase LEB %d:%d", vol->vol_id, lnum);
 
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
@@ -542,7 +542,7 @@ int ubi_leb_unmap(struct ubi_volume_desc *desc, int lnum)
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
 
-	dbg_msg("unmap LEB %d:%d", vol->vol_id, lnum);
+	dbg_gen("unmap LEB %d:%d", vol->vol_id, lnum);
 
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
@@ -579,7 +579,7 @@ int ubi_leb_map(struct ubi_volume_desc *desc, int lnum, int dtype)
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
 
-	dbg_msg("unmap LEB %d:%d", vol->vol_id, lnum);
+	dbg_gen("unmap LEB %d:%d", vol->vol_id, lnum);
 
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
@@ -621,7 +621,7 @@ int ubi_is_mapped(struct ubi_volume_desc *desc, int lnum)
 {
 	struct ubi_volume *vol = desc->vol;
 
-	dbg_msg("test LEB %d:%d", vol->vol_id, lnum);
+	dbg_gen("test LEB %d:%d", vol->vol_id, lnum);
 
 	if (lnum < 0 || lnum >= vol->reserved_pebs)
 		return -EINVAL;
@@ -632,3 +632,27 @@ int ubi_is_mapped(struct ubi_volume_desc *desc, int lnum)
 	return vol->eba_tbl[lnum] >= 0;
 }
 EXPORT_SYMBOL_GPL(ubi_is_mapped);
+
+/**
+ * ubi_sync - synchronize UBI device buffers.
+ * @ubi_num: UBI device to synchronize
+ *
+ * The underlying MTD device may cache data in hardware or in software. This
+ * function ensures the caches are flushed. Returns zero in case of success and
+ * a negative error code in case of failure.
+ */
+int ubi_sync(int ubi_num)
+{
+	struct ubi_device *ubi;
+
+	ubi = ubi_get_device(ubi_num);
+	if (!ubi)
+		return -ENODEV;
+
+	if (ubi->mtd->sync)
+		ubi->mtd->sync(ubi->mtd);
+
+	ubi_put_device(ubi);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ubi_sync);
diff --git a/drivers/mtd/ubi/misc.c b/drivers/mtd/ubi/misc.c
index 93e05281201..22ad3140294 100644
--- a/drivers/mtd/ubi/misc.c
+++ b/drivers/mtd/ubi/misc.c
@@ -37,7 +37,7 @@ int ubi_calc_data_len(const struct ubi_device *ubi, const void *buf,
 {
 	int i;
 
-	ubi_assert(length % ubi->min_io_size == 0);
+	ubi_assert(!(length & (ubi->min_io_size - 1)));
 
 	for (i = length - 1; i >= 0; i--)
 		if (((const uint8_t *)buf)[i] != 0xFF)
diff --git a/drivers/mtd/ubi/scan.c b/drivers/mtd/ubi/scan.c
index 96d410e106a..967bb4406df 100644
--- a/drivers/mtd/ubi/scan.c
+++ b/drivers/mtd/ubi/scan.c
@@ -19,9 +19,9 @@
  */
 
 /*
- * UBI scanning unit.
+ * UBI scanning sub-system.
  *
- * This unit is responsible for scanning the flash media, checking UBI
+ * This sub-system is responsible for scanning the flash media, checking UBI
  * headers and providing complete information about the UBI flash image.
  *
  * The scanning information is represented by a &struct ubi_scan_info' object.
@@ -93,8 +93,7 @@ static int add_to_list(struct ubi_scan_info *si, int pnum, int ec,
 }
 
 /**
- * validate_vid_hdr - check that volume identifier header is correct and
- * consistent.
+ * validate_vid_hdr - check volume identifier header.
  * @vid_hdr: the volume identifier header to check
  * @sv: information about the volume this logical eraseblock belongs to
  * @pnum: physical eraseblock number the VID header came from
@@ -103,7 +102,7 @@ static int add_to_list(struct ubi_scan_info *si, int pnum, int ec,
  * non-zero if an inconsistency was found and zero if not.
  *
  * Note, UBI does sanity check of everything it reads from the flash media.
- * Most of the checks are done in the I/O unit. Here we check that the
+ * Most of the checks are done in the I/O sub-system. Here we check that the
  * information in the VID header is consistent to the information in other VID
  * headers of the same volume.
  */
@@ -247,45 +246,21 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 	struct ubi_vid_hdr *vh = NULL;
 	unsigned long long sqnum2 = be64_to_cpu(vid_hdr->sqnum);
 
-	if (seb->sqnum == 0 && sqnum2 == 0) {
-		long long abs, v1 = seb->leb_ver, v2 = be32_to_cpu(vid_hdr->leb_ver);
-
+	if (sqnum2 == seb->sqnum) {
 		/*
-		 * UBI constantly increases the logical eraseblock version
-		 * number and it can overflow. Thus, we have to bear in mind
-		 * that versions that are close to %0xFFFFFFFF are less then
-		 * versions that are close to %0.
-		 *
-		 * The UBI WL unit guarantees that the number of pending tasks
-		 * is not greater then %0x7FFFFFFF. So, if the difference
-		 * between any two versions is greater or equivalent to
-		 * %0x7FFFFFFF, there was an overflow and the logical
-		 * eraseblock with lower version is actually newer then the one
-		 * with higher version.
-		 *
-		 * FIXME: but this is anyway obsolete and will be removed at
-		 * some point.
+		 * This must be a really ancient UBI image which has been
+		 * created before sequence numbers support has been added. At
+		 * that times we used 32-bit LEB versions stored in logical
+		 * eraseblocks. That was before UBI got into mainline. We do not
+		 * support these images anymore. Well, those images will work
+		 * still work, but only if no unclean reboots happened.
 		 */
-		dbg_bld("using old crappy leb_ver stuff");
-
-		if (v1 == v2) {
-			ubi_err("PEB %d and PEB %d have the same version %lld",
-				seb->pnum, pnum, v1);
-			return -EINVAL;
-		}
-
-		abs = v1 - v2;
-		if (abs < 0)
-			abs = -abs;
+		ubi_err("unsupported on-flash UBI format\n");
+		return -EINVAL;
+	}
 
-		if (abs < 0x7FFFFFFF)
-			/* Non-overflow situation */
-			second_is_newer = (v2 > v1);
-		else
-			second_is_newer = (v2 < v1);
-	} else
-		/* Obviously the LEB with lower sequence counter is older */
-		second_is_newer = sqnum2 > seb->sqnum;
+	/* Obviously the LEB with lower sequence counter is older */
+	second_is_newer = !!(sqnum2 > seb->sqnum);
 
 	/*
 	 * Now we know which copy is newer. If the copy flag of the PEB with
@@ -293,7 +268,7 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 	 * check data CRC. For the second PEB we already have the VID header,
 	 * for the first one - we'll need to re-read it from flash.
 	 *
-	 * FIXME: this may be optimized so that we wouldn't read twice.
+	 * Note: this may be optimized so that we wouldn't read twice.
 	 */
 
 	if (second_is_newer) {
@@ -379,8 +354,7 @@ out_free_vidh:
 }
 
 /**
- * ubi_scan_add_used - add information about a physical eraseblock to the
- * scanning information.
+ * ubi_scan_add_used - add physical eraseblock to the scanning information.
  * @ubi: UBI device description object
  * @si: scanning information
  * @pnum: the physical eraseblock number
@@ -400,7 +374,6 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 		      int bitflips)
 {
 	int err, vol_id, lnum;
-	uint32_t leb_ver;
 	unsigned long long sqnum;
 	struct ubi_scan_volume *sv;
 	struct ubi_scan_leb *seb;
@@ -409,10 +382,9 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 	vol_id = be32_to_cpu(vid_hdr->vol_id);
 	lnum = be32_to_cpu(vid_hdr->lnum);
 	sqnum = be64_to_cpu(vid_hdr->sqnum);
-	leb_ver = be32_to_cpu(vid_hdr->leb_ver);
 
-	dbg_bld("PEB %d, LEB %d:%d, EC %d, sqnum %llu, ver %u, bitflips %d",
-		pnum, vol_id, lnum, ec, sqnum, leb_ver, bitflips);
+	dbg_bld("PEB %d, LEB %d:%d, EC %d, sqnum %llu, bitflips %d",
+		pnum, vol_id, lnum, ec, sqnum, bitflips);
 
 	sv = add_volume(si, vol_id, pnum, vid_hdr);
 	if (IS_ERR(sv) < 0)
@@ -445,25 +417,20 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 		 */
 
 		dbg_bld("this LEB already exists: PEB %d, sqnum %llu, "
-			"LEB ver %u, EC %d", seb->pnum, seb->sqnum,
-			seb->leb_ver, seb->ec);
-
-		/*
-		 * Make sure that the logical eraseblocks have different
-		 * versions. Otherwise the image is bad.
-		 */
-		if (seb->leb_ver == leb_ver && leb_ver != 0) {
-			ubi_err("two LEBs with same version %u", leb_ver);
-			ubi_dbg_dump_seb(seb, 0);
-			ubi_dbg_dump_vid_hdr(vid_hdr);
-			return -EINVAL;
-		}
+			"EC %d", seb->pnum, seb->sqnum, seb->ec);
 
 		/*
 		 * Make sure that the logical eraseblocks have different
 		 * sequence numbers. Otherwise the image is bad.
 		 *
-		 * FIXME: remove 'sqnum != 0' check when leb_ver is removed.
+		 * However, if the sequence number is zero, we assume it must
+		 * be an ancient UBI image from the era when UBI did not have
+		 * sequence numbers. We still can attach these images, unless
+		 * there is a need to distinguish between old and new
+		 * eraseblocks, in which case we'll refuse the image in
+		 * 'compare_lebs()'. In other words, we attach old clean
+		 * images, but refuse attaching old images with duplicated
+		 * logical eraseblocks because there was an unclean reboot.
 		 */
 		if (seb->sqnum == sqnum && sqnum != 0) {
 			ubi_err("two LEBs with same sequence number %llu",
@@ -503,7 +470,6 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 			seb->pnum = pnum;
 			seb->scrub = ((cmp_res & 2) || bitflips);
 			seb->sqnum = sqnum;
-			seb->leb_ver = leb_ver;
 
 			if (sv->highest_lnum == lnum)
 				sv->last_data_size =
@@ -540,7 +506,6 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 	seb->lnum = lnum;
 	seb->sqnum = sqnum;
 	seb->scrub = bitflips;
-	seb->leb_ver = leb_ver;
 
 	if (sv->highest_lnum <= lnum) {
 		sv->highest_lnum = lnum;
@@ -554,8 +519,7 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 }
 
 /**
- * ubi_scan_find_sv - find information about a particular volume in the
- * scanning information.
+ * ubi_scan_find_sv - find volume in the scanning information.
  * @si: scanning information
  * @vol_id: the requested volume ID
  *
@@ -584,8 +548,7 @@ struct ubi_scan_volume *ubi_scan_find_sv(const struct ubi_scan_info *si,
 }
 
 /**
- * ubi_scan_find_seb - find information about a particular logical
- * eraseblock in the volume scanning information.
+ * ubi_scan_find_seb - find LEB in the volume scanning information.
  * @sv: a pointer to the volume scanning information
  * @lnum: the requested logical eraseblock
  *
@@ -645,9 +608,9 @@ void ubi_scan_rm_volume(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
  *
  * This function erases physical eraseblock 'pnum', and writes the erase
  * counter header to it. This function should only be used on UBI device
- * initialization stages, when the EBA unit had not been yet initialized. This
- * function returns zero in case of success and a negative error code in case
- * of failure.
+ * initialization stages, when the EBA sub-system had not been yet initialized.
+ * This function returns zero in case of success and a negative error code in
+ * case of failure.
  */
 int ubi_scan_erase_peb(struct ubi_device *ubi, const struct ubi_scan_info *si,
 		       int pnum, int ec)
@@ -687,9 +650,10 @@ out_free:
  * @si: scanning information
  *
  * This function returns a free physical eraseblock. It is supposed to be
- * called on the UBI initialization stages when the wear-leveling unit is not
- * initialized yet. This function picks a physical eraseblocks from one of the
- * lists, writes the EC header if it is needed, and removes it from the list.
+ * called on the UBI initialization stages when the wear-leveling sub-system is
+ * not initialized yet. This function picks a physical eraseblocks from one of
+ * the lists, writes the EC header if it is needed, and removes it from the
+ * list.
  *
  * This function returns scanning physical eraseblock information in case of
  * success and an error code in case of failure.
@@ -742,8 +706,7 @@ struct ubi_scan_leb *ubi_scan_get_free_peb(struct ubi_device *ubi,
 }
 
 /**
- * process_eb - read UBI headers, check them and add corresponding data
- * to the scanning information.
+ * process_eb - read, check UBI headers, and add them to scanning information.
  * @ubi: UBI device description object
  * @si: scanning information
  * @pnum: the physical eraseblock number
@@ -751,7 +714,8 @@ struct ubi_scan_leb *ubi_scan_get_free_peb(struct ubi_device *ubi,
  * This function returns a zero if the physical eraseblock was successfully
  * handled and a negative error code in case of failure.
  */
-static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si, int pnum)
+static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
+		      int pnum)
 {
 	long long uninitialized_var(ec);
 	int err, bitflips = 0, vol_id, ec_corr = 0;
@@ -764,8 +728,9 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si, int pnum
 		return err;
 	else if (err) {
 		/*
-		 * FIXME: this is actually duty of the I/O unit to initialize
-		 * this, but MTD does not provide enough information.
+		 * FIXME: this is actually duty of the I/O sub-system to
+		 * initialize this, but MTD does not provide enough
+		 * information.
 		 */
 		si->bad_peb_count += 1;
 		return 0;
@@ -930,7 +895,7 @@ struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
 	for (pnum = 0; pnum < ubi->peb_count; pnum++) {
 		cond_resched();
 
-		dbg_msg("process PEB %d", pnum);
+		dbg_gen("process PEB %d", pnum);
 		err = process_eb(ubi, si, pnum);
 		if (err < 0)
 			goto out_vidh;
@@ -1079,8 +1044,7 @@ void ubi_scan_destroy_si(struct ubi_scan_info *si)
 #ifdef CONFIG_MTD_UBI_DEBUG_PARANOID
 
 /**
- * paranoid_check_si - check if the scanning information is correct and
- * consistent.
+ * paranoid_check_si - check the scanning information.
  * @ubi: UBI device description object
  * @si: scanning information
  *
@@ -1265,11 +1229,6 @@ static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
 				ubi_err("bad data_pad %d", sv->data_pad);
 				goto bad_vid_hdr;
 			}
-
-			if (seb->leb_ver != be32_to_cpu(vidh->leb_ver)) {
-				ubi_err("bad leb_ver %u", seb->leb_ver);
-				goto bad_vid_hdr;
-			}
 		}
 
 		if (!last_seb)
@@ -1299,8 +1258,7 @@ static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
 		if (err < 0) {
 			kfree(buf);
 			return err;
-		}
-		else if (err)
+		} else if (err)
 			buf[pnum] = 1;
 	}
 
diff --git a/drivers/mtd/ubi/scan.h b/drivers/mtd/ubi/scan.h
index 966b9b682a4..61df208e2f2 100644
--- a/drivers/mtd/ubi/scan.h
+++ b/drivers/mtd/ubi/scan.h
@@ -34,7 +34,6 @@
  * @u: unions RB-tree or @list links
  * @u.rb: link in the per-volume RB-tree of &struct ubi_scan_leb objects
  * @u.list: link in one of the eraseblock lists
- * @leb_ver: logical eraseblock version (obsolete)
  *
  * One object of this type is allocated for each physical eraseblock during
  * scanning.
@@ -49,7 +48,6 @@ struct ubi_scan_leb {
 		struct rb_node rb;
 		struct list_head list;
 	} u;
-	uint32_t leb_ver;
 };
 
 /**
@@ -59,16 +57,16 @@ struct ubi_scan_leb {
  * @leb_count: number of logical eraseblocks in this volume
  * @vol_type: volume type
  * @used_ebs: number of used logical eraseblocks in this volume (only for
- * static volumes)
+ *            static volumes)
  * @last_data_size: amount of data in the last logical eraseblock of this
- * volume (always equivalent to the usable logical eraseblock size in case of
- * dynamic volumes)
+ *                  volume (always equivalent to the usable logical eraseblock
+ *                  size in case of dynamic volumes)
  * @data_pad: how many bytes at the end of logical eraseblocks of this volume
- * are not used (due to volume alignment)
+ *            are not used (due to volume alignment)
  * @compat: compatibility flags of this volume
  * @rb: link in the volume RB-tree
  * @root: root of the RB-tree containing all the eraseblock belonging to this
- * volume (&struct ubi_scan_leb objects)
+ *        volume (&struct ubi_scan_leb objects)
  *
  * One object of this type is allocated for each volume during scanning.
  */
@@ -92,8 +90,8 @@ struct ubi_scan_volume {
  * @free: list of free physical eraseblocks
  * @erase: list of physical eraseblocks which have to be erased
  * @alien: list of physical eraseblocks which should not be used by UBI (e.g.,
+ *         those belonging to "preserve"-compatible internal volumes)
  * @bad_peb_count: count of bad physical eraseblocks
- * those belonging to "preserve"-compatible internal volumes)
  * @vols_found: number of volumes found during scanning
  * @highest_vol_id: highest volume ID
  * @alien_peb_count: count of physical eraseblocks in the @alien list
@@ -106,8 +104,8 @@ struct ubi_scan_volume {
  * @ec_count: a temporary variable used when calculating @mean_ec
  *
  * This data structure contains the result of scanning and may be used by other
- * UBI units to build final UBI data structures, further error-recovery and so
- * on.
+ * UBI sub-systems to build final UBI data structures, further error-recovery
+ * and so on.
  */
 struct ubi_scan_info {
 	struct rb_root volumes;
@@ -132,8 +130,7 @@ struct ubi_device;
 struct ubi_vid_hdr;
 
 /*
- * ubi_scan_move_to_list - move a physical eraseblock from the volume tree to a
- * list.
+ * ubi_scan_move_to_list - move a PEB from the volume tree to a list.
  *
  * @sv: volume scanning information
  * @seb: scanning eraseblock infprmation
diff --git a/drivers/mtd/ubi/ubi-media.h b/drivers/mtd/ubi/ubi-media.h
index c3185d9fd04..2ad94040905 100644
--- a/drivers/mtd/ubi/ubi-media.h
+++ b/drivers/mtd/ubi/ubi-media.h
@@ -98,10 +98,11 @@ enum {
  * Compatibility constants used by internal volumes.
  *
  * @UBI_COMPAT_DELETE: delete this internal volume before anything is written
- * to the flash
+ *                     to the flash
  * @UBI_COMPAT_RO: attach this device in read-only mode
  * @UBI_COMPAT_PRESERVE: preserve this internal volume - do not touch its
- * physical eraseblocks, don't allow the wear-leveling unit to move them
+ *                       physical eraseblocks, don't allow the wear-leveling
+ *                       sub-system to move them
  * @UBI_COMPAT_REJECT: reject this UBI image
  */
 enum {
@@ -123,7 +124,7 @@ enum {
  * struct ubi_ec_hdr - UBI erase counter header.
  * @magic: erase counter header magic number (%UBI_EC_HDR_MAGIC)
  * @version: version of UBI implementation which is supposed to accept this
- * UBI image
+ *           UBI image
  * @padding1: reserved for future, zeroes
  * @ec: the erase counter
  * @vid_hdr_offset: where the VID header starts
@@ -159,24 +160,23 @@ struct ubi_ec_hdr {
  * struct ubi_vid_hdr - on-flash UBI volume identifier header.
  * @magic: volume identifier header magic number (%UBI_VID_HDR_MAGIC)
  * @version: UBI implementation version which is supposed to accept this UBI
- * image (%UBI_VERSION)
+ *           image (%UBI_VERSION)
  * @vol_type: volume type (%UBI_VID_DYNAMIC or %UBI_VID_STATIC)
  * @copy_flag: if this logical eraseblock was copied from another physical
- * eraseblock (for wear-leveling reasons)
+ *             eraseblock (for wear-leveling reasons)
  * @compat: compatibility of this volume (%0, %UBI_COMPAT_DELETE,
- * %UBI_COMPAT_IGNORE, %UBI_COMPAT_PRESERVE, or %UBI_COMPAT_REJECT)
+ *          %UBI_COMPAT_IGNORE, %UBI_COMPAT_PRESERVE, or %UBI_COMPAT_REJECT)
  * @vol_id: ID of this volume
  * @lnum: logical eraseblock number
- * @leb_ver: version of this logical eraseblock (IMPORTANT: obsolete, to be
- * removed, kept only for not breaking older UBI users)
+ * @padding1: reserved for future, zeroes
  * @data_size: how many bytes of data this logical eraseblock contains
  * @used_ebs: total number of used logical eraseblocks in this volume
  * @data_pad: how many bytes at the end of this physical eraseblock are not
- * used
+ *            used
  * @data_crc: CRC checksum of the data stored in this logical eraseblock
- * @padding1: reserved for future, zeroes
- * @sqnum: sequence number
  * @padding2: reserved for future, zeroes
+ * @sqnum: sequence number
+ * @padding3: reserved for future, zeroes
  * @hdr_crc: volume identifier header CRC checksum
  *
  * The @sqnum is the value of the global sequence counter at the time when this
@@ -224,10 +224,6 @@ struct ubi_ec_hdr {
  * checksum is correct, this physical eraseblock is selected (P1). Otherwise
  * the older one (P) is selected.
  *
- * Note, there is an obsolete @leb_ver field which was used instead of @sqnum
- * in the past. But it is not used anymore and we keep it in order to be able
- * to deal with old UBI images. It will be removed at some point.
- *
  * There are 2 sorts of volumes in UBI: user volumes and internal volumes.
  * Internal volumes are not seen from outside and are used for various internal
  * UBI purposes. In this implementation there is only one internal volume - the
@@ -248,9 +244,9 @@ struct ubi_ec_hdr {
  * The @data_crc field contains the CRC checksum of the contents of the logical
  * eraseblock if this is a static volume. In case of dynamic volumes, it does
  * not contain the CRC checksum as a rule. The only exception is when the
- * data of the physical eraseblock was moved by the wear-leveling unit, then
- * the wear-leveling unit calculates the data CRC and stores it in the
- * @data_crc field. And of course, the @copy_flag is %in this case.
+ * data of the physical eraseblock was moved by the wear-leveling sub-system,
+ * then the wear-leveling sub-system calculates the data CRC and stores it in
+ * the @data_crc field. And of course, the @copy_flag is %in this case.
  *
  * The @data_size field is used only for static volumes because UBI has to know
  * how many bytes of data are stored in this eraseblock. For dynamic volumes,
@@ -277,14 +273,14 @@ struct ubi_vid_hdr {
 	__u8    compat;
 	__be32  vol_id;
 	__be32  lnum;
-	__be32  leb_ver; /* obsolete, to be removed, don't use */
+	__u8    padding1[4];
 	__be32  data_size;
 	__be32  used_ebs;
 	__be32  data_pad;
 	__be32  data_crc;
-	__u8    padding1[4];
+	__u8    padding2[4];
 	__be64  sqnum;
-	__u8    padding2[12];
+	__u8    padding3[12];
 	__be32  hdr_crc;
 } __attribute__ ((packed));
 
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 67dcbd11c15..1c3fa18c26a 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -74,15 +74,15 @@
 #define UBI_IO_RETRIES 3
 
 /*
- * Error codes returned by the I/O unit.
+ * Error codes returned by the I/O sub-system.
  *
  * UBI_IO_PEB_EMPTY: the physical eraseblock is empty, i.e. it contains only
- * 0xFF bytes
+ *                   %0xFF bytes
  * UBI_IO_PEB_FREE: the physical eraseblock is free, i.e. it contains only a
- * valid erase counter header, and the rest are %0xFF bytes
+ *                  valid erase counter header, and the rest are %0xFF bytes
  * UBI_IO_BAD_EC_HDR: the erase counter header is corrupted (bad magic or CRC)
  * UBI_IO_BAD_VID_HDR: the volume identifier header is corrupted (bad magic or
- * CRC)
+ *                     CRC)
  * UBI_IO_BITFLIPS: bit-flips were detected and corrected
  */
 enum {
@@ -99,9 +99,9 @@ enum {
  * @ec: erase counter
  * @pnum: physical eraseblock number
  *
- * This data structure is used in the WL unit. Each physical eraseblock has a
- * corresponding &struct wl_entry object which may be kept in different
- * RB-trees. See WL unit for details.
+ * This data structure is used in the WL sub-system. Each physical eraseblock
+ * has a corresponding &struct wl_entry object which may be kept in different
+ * RB-trees. See WL sub-system for details.
  */
 struct ubi_wl_entry {
 	struct rb_node rb;
@@ -118,10 +118,10 @@ struct ubi_wl_entry {
  * @mutex: read/write mutex to implement read/write access serialization to
  *         the (@vol_id, @lnum) logical eraseblock
  *
- * This data structure is used in the EBA unit to implement per-LEB locking.
- * When a logical eraseblock is being locked - corresponding
+ * This data structure is used in the EBA sub-system to implement per-LEB
+ * locking. When a logical eraseblock is being locked - corresponding
  * &struct ubi_ltree_entry object is inserted to the lock tree (@ubi->ltree).
- * See EBA unit for details.
+ * See EBA sub-system for details.
  */
 struct ubi_ltree_entry {
 	struct rb_node rb;
@@ -131,6 +131,27 @@ struct ubi_ltree_entry {
 	struct rw_semaphore mutex;
 };
 
+/**
+ * struct ubi_rename_entry - volume re-name description data structure.
+ * @new_name_len: new volume name length
+ * @new_name: new volume name
+ * @remove: if not zero, this volume should be removed, not re-named
+ * @desc: descriptor of the volume
+ * @list: links re-name entries into a list
+ *
+ * This data structure is utilized in the multiple volume re-name code. Namely,
+ * UBI first creates a list of &struct ubi_rename_entry objects from the
+ * &struct ubi_rnvol_req request object, and then utilizes this list to do all
+ * the job.
+ */
+struct ubi_rename_entry {
+	int new_name_len;
+	char new_name[UBI_VOL_NAME_MAX + 1];
+	int remove;
+	struct ubi_volume_desc *desc;
+	struct list_head list;
+};
+
 struct ubi_volume_desc;
 
 /**
@@ -206,7 +227,7 @@ struct ubi_volume {
 	int alignment;
 	int data_pad;
 	int name_len;
-	char name[UBI_VOL_NAME_MAX+1];
+	char name[UBI_VOL_NAME_MAX + 1];
 
 	int upd_ebs;
 	int ch_lnum;
@@ -225,7 +246,7 @@ struct ubi_volume {
 #ifdef CONFIG_MTD_UBI_GLUEBI
 	/*
 	 * Gluebi-related stuff may be compiled out.
-	 * TODO: this should not be built into UBI but should be a separate
+	 * Note: this should not be built into UBI but should be a separate
 	 * ubimtd driver which works on top of UBI and emulates MTD devices.
 	 */
 	struct ubi_volume_desc *gluebi_desc;
@@ -235,8 +256,7 @@ struct ubi_volume {
 };
 
 /**
- * struct ubi_volume_desc - descriptor of the UBI volume returned when it is
- * opened.
+ * struct ubi_volume_desc - UBI volume descriptor returned when it is opened.
  * @vol: reference to the corresponding volume description object
  * @mode: open mode (%UBI_READONLY, %UBI_READWRITE, or %UBI_EXCLUSIVE)
  */
@@ -273,7 +293,7 @@ struct ubi_wl_entry;
  * @vtbl_size: size of the volume table in bytes
  * @vtbl: in-RAM volume table copy
  * @volumes_mutex: protects on-flash volume table and serializes volume
- *                 changes, like creation, deletion, update, resize
+ *                 changes, like creation, deletion, update, re-size and re-name
  *
  * @max_ec: current highest erase counter value
  * @mean_ec: current mean erase counter value
@@ -293,6 +313,7 @@ struct ubi_wl_entry;
  *           @move_to, @move_to_put @erase_pending, @wl_scheduled, and @works
  *           fields
  * @move_mutex: serializes eraseblock moves
+ * @work_sem: sycnhronizes the WL worker with use tasks
  * @wl_scheduled: non-zero if the wear-leveling was scheduled
  * @lookuptbl: a table to quickly find a &struct ubi_wl_entry object for any
  *             physical eraseblock
@@ -316,11 +337,11 @@ struct ubi_wl_entry;
  * @ro_mode: if the UBI device is in read-only mode
  * @leb_size: logical eraseblock size
  * @leb_start: starting offset of logical eraseblocks within physical
- * eraseblocks
+ *             eraseblocks
  * @ec_hdr_alsize: size of the EC header aligned to @hdrs_min_io_size
  * @vid_hdr_alsize: size of the VID header aligned to @hdrs_min_io_size
  * @vid_hdr_offset: starting offset of the volume identifier header (might be
- * unaligned)
+ *                  unaligned)
  * @vid_hdr_aloffset: starting offset of the VID header aligned to
  * @hdrs_min_io_size
  * @vid_hdr_shift: contains @vid_hdr_offset - @vid_hdr_aloffset
@@ -331,6 +352,8 @@ struct ubi_wl_entry;
  * @peb_buf1: a buffer of PEB size used for different purposes
  * @peb_buf2: another buffer of PEB size used for different purposes
  * @buf_mutex: proptects @peb_buf1 and @peb_buf2
+ * @ckvol_mutex: serializes static volume checking when opening
+ * @mult_mutex: serializes operations on multiple volumes, like re-nameing
  * @dbg_peb_buf: buffer of PEB size used for debugging
  * @dbg_buf_mutex: proptects @dbg_peb_buf
  */
@@ -356,16 +379,16 @@ struct ubi_device {
 	struct mutex volumes_mutex;
 
 	int max_ec;
-	/* TODO: mean_ec is not updated run-time, fix */
+	/* Note, mean_ec is not updated run-time - should be fixed */
 	int mean_ec;
 
-	/* EBA unit's stuff */
+	/* EBA sub-system's stuff */
 	unsigned long long global_sqnum;
 	spinlock_t ltree_lock;
 	struct rb_root ltree;
 	struct mutex alc_mutex;
 
-	/* Wear-leveling unit's stuff */
+	/* Wear-leveling sub-system's stuff */
 	struct rb_root used;
 	struct rb_root free;
 	struct rb_root scrub;
@@ -388,7 +411,7 @@ struct ubi_device {
 	int thread_enabled;
 	char bgt_name[sizeof(UBI_BGT_NAME_PATTERN)+2];
 
-	/* I/O unit's stuff */
+	/* I/O sub-system's stuff */
 	long long flash_size;
 	int peb_count;
 	int peb_size;
@@ -411,6 +434,7 @@ struct ubi_device {
 	void *peb_buf2;
 	struct mutex buf_mutex;
 	struct mutex ckvol_mutex;
+	struct mutex mult_mutex;
 #ifdef CONFIG_MTD_UBI_DEBUG
 	void *dbg_peb_buf;
 	struct mutex dbg_buf_mutex;
@@ -427,12 +451,15 @@ extern struct mutex ubi_devices_mutex;
 /* vtbl.c */
 int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 			   struct ubi_vtbl_record *vtbl_rec);
+int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
+			    struct list_head *rename_list);
 int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si);
 
 /* vmt.c */
 int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req);
-int ubi_remove_volume(struct ubi_volume_desc *desc);
+int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl);
 int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs);
+int ubi_rename_volumes(struct ubi_device *ubi, struct list_head *rename_list);
 int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol);
 void ubi_free_volume(struct ubi_device *ubi, struct ubi_volume *vol);
 
@@ -447,7 +474,8 @@ int ubi_more_leb_change_data(struct ubi_device *ubi, struct ubi_volume *vol,
 			     const void __user *buf, int count);
 
 /* misc.c */
-int ubi_calc_data_len(const struct ubi_device *ubi, const void *buf, int length);
+int ubi_calc_data_len(const struct ubi_device *ubi, const void *buf,
+		      int length);
 int ubi_check_volume(struct ubi_device *ubi, int vol_id);
 void ubi_calculate_reserved(struct ubi_device *ubi);
 
@@ -477,7 +505,6 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		     struct ubi_vid_hdr *vid_hdr);
 int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si);
-void ubi_eba_close(const struct ubi_device *ubi);
 
 /* wl.c */
 int ubi_wl_get_peb(struct ubi_device *ubi, int dtype);
diff --git a/drivers/mtd/ubi/upd.c b/drivers/mtd/ubi/upd.c
index ddaa1a56cc6..8b89cc18ff0 100644
--- a/drivers/mtd/ubi/upd.c
+++ b/drivers/mtd/ubi/upd.c
@@ -39,7 +39,7 @@
  */
 
 #include <linux/err.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <asm/div64.h>
 #include "ubi.h"
 
@@ -56,11 +56,11 @@ static int set_update_marker(struct ubi_device *ubi, struct ubi_volume *vol)
 	int err;
 	struct ubi_vtbl_record vtbl_rec;
 
-	dbg_msg("set update marker for volume %d", vol->vol_id);
+	dbg_gen("set update marker for volume %d", vol->vol_id);
 
 	if (vol->upd_marker) {
 		ubi_assert(ubi->vtbl[vol->vol_id].upd_marker);
-		dbg_msg("already set");
+		dbg_gen("already set");
 		return 0;
 	}
 
@@ -92,7 +92,7 @@ static int clear_update_marker(struct ubi_device *ubi, struct ubi_volume *vol,
 	uint64_t tmp;
 	struct ubi_vtbl_record vtbl_rec;
 
-	dbg_msg("clear update marker for volume %d", vol->vol_id);
+	dbg_gen("clear update marker for volume %d", vol->vol_id);
 
 	memcpy(&vtbl_rec, &ubi->vtbl[vol->vol_id],
 	       sizeof(struct ubi_vtbl_record));
@@ -133,7 +133,7 @@ int ubi_start_update(struct ubi_device *ubi, struct ubi_volume *vol,
 	int i, err;
 	uint64_t tmp;
 
-	dbg_msg("start update of volume %d, %llu bytes", vol->vol_id, bytes);
+	dbg_gen("start update of volume %d, %llu bytes", vol->vol_id, bytes);
 	ubi_assert(!vol->updating && !vol->changing_leb);
 	vol->updating = 1;
 
@@ -183,7 +183,7 @@ int ubi_start_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 {
 	ubi_assert(!vol->updating && !vol->changing_leb);
 
-	dbg_msg("start changing LEB %d:%d, %u bytes",
+	dbg_gen("start changing LEB %d:%d, %u bytes",
 		vol->vol_id, req->lnum, req->bytes);
 	if (req->bytes == 0)
 		return ubi_eba_atomic_leb_change(ubi, vol, req->lnum, NULL, 0,
@@ -237,16 +237,17 @@ static int write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	int err;
 
 	if (vol->vol_type == UBI_DYNAMIC_VOLUME) {
-		len = ALIGN(len, ubi->min_io_size);
-		memset(buf + len, 0xFF, len - len);
+		int l = ALIGN(len, ubi->min_io_size);
 
-		len = ubi_calc_data_len(ubi, buf, len);
+		memset(buf + len, 0xFF, l - len);
+		len = ubi_calc_data_len(ubi, buf, l);
 		if (len == 0) {
-			dbg_msg("all %d bytes contain 0xFF - skip", len);
+			dbg_gen("all %d bytes contain 0xFF - skip", len);
 			return 0;
 		}
 
-		err = ubi_eba_write_leb(ubi, vol, lnum, buf, 0, len, UBI_UNKNOWN);
+		err = ubi_eba_write_leb(ubi, vol, lnum, buf, 0, len,
+					UBI_UNKNOWN);
 	} else {
 		/*
 		 * When writing static volume, and this is the last logical
@@ -267,6 +268,7 @@ static int write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 
 /**
  * ubi_more_update_data - write more update data.
+ * @ubi: UBI device description object
  * @vol: volume description object
  * @buf: write data (user-space memory buffer)
  * @count: how much bytes to write
@@ -283,7 +285,7 @@ int ubi_more_update_data(struct ubi_device *ubi, struct ubi_volume *vol,
 	uint64_t tmp;
 	int lnum, offs, err = 0, len, to_write = count;
 
-	dbg_msg("write %d of %lld bytes, %lld already passed",
+	dbg_gen("write %d of %lld bytes, %lld already passed",
 		count, vol->upd_bytes, vol->upd_received);
 
 	if (ubi->ro_mode)
@@ -384,6 +386,7 @@ int ubi_more_update_data(struct ubi_device *ubi, struct ubi_volume *vol,
 
 /**
  * ubi_more_leb_change_data - accept more data for atomic LEB change.
+ * @ubi: UBI device description object
  * @vol: volume description object
  * @buf: write data (user-space memory buffer)
  * @count: how much bytes to write
@@ -400,7 +403,7 @@ int ubi_more_leb_change_data(struct ubi_device *ubi, struct ubi_volume *vol,
 {
 	int err;
 
-	dbg_msg("write %d of %lld bytes, %lld already passed",
+	dbg_gen("write %d of %lld bytes, %lld already passed",
 		count, vol->upd_bytes, vol->upd_received);
 
 	if (ubi->ro_mode)
@@ -418,7 +421,8 @@ int ubi_more_leb_change_data(struct ubi_device *ubi, struct ubi_volume *vol,
 	if (vol->upd_received == vol->upd_bytes) {
 		int len = ALIGN((int)vol->upd_bytes, ubi->min_io_size);
 
-		memset(vol->upd_buf + vol->upd_bytes, 0xFF, len - vol->upd_bytes);
+		memset(vol->upd_buf + vol->upd_bytes, 0xFF,
+		       len - vol->upd_bytes);
 		len = ubi_calc_data_len(ubi, vol->upd_buf, len);
 		err = ubi_eba_atomic_leb_change(ubi, vol, vol->ch_lnum,
 						vol->upd_buf, len, UBI_UNKNOWN);
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 5be58d85c63..3531ca9a1e2 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -28,9 +28,9 @@
 #include "ubi.h"
 
 #ifdef CONFIG_MTD_UBI_DEBUG_PARANOID
-static void paranoid_check_volumes(struct ubi_device *ubi);
+static int paranoid_check_volumes(struct ubi_device *ubi);
 #else
-#define paranoid_check_volumes(ubi)
+#define paranoid_check_volumes(ubi) 0
 #endif
 
 static ssize_t vol_attribute_show(struct device *dev,
@@ -127,6 +127,7 @@ static void vol_release(struct device *dev)
 {
 	struct ubi_volume *vol = container_of(dev, struct ubi_volume, dev);
 
+	kfree(vol->eba_tbl);
 	kfree(vol);
 }
 
@@ -201,7 +202,7 @@ static void volume_sysfs_close(struct ubi_volume *vol)
  */
 int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 {
-	int i, err, vol_id = req->vol_id, dont_free = 0;
+	int i, err, vol_id = req->vol_id, do_free = 1;
 	struct ubi_volume *vol;
 	struct ubi_vtbl_record vtbl_rec;
 	uint64_t bytes;
@@ -217,7 +218,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	spin_lock(&ubi->volumes_lock);
 	if (vol_id == UBI_VOL_NUM_AUTO) {
 		/* Find unused volume ID */
-		dbg_msg("search for vacant volume ID");
+		dbg_gen("search for vacant volume ID");
 		for (i = 0; i < ubi->vtbl_slots; i++)
 			if (!ubi->volumes[i]) {
 				vol_id = i;
@@ -232,7 +233,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 		req->vol_id = vol_id;
 	}
 
-	dbg_msg("volume ID %d, %llu bytes, type %d, name %s",
+	dbg_gen("volume ID %d, %llu bytes, type %d, name %s",
 		vol_id, (unsigned long long)req->bytes,
 		(int)req->vol_type, req->name);
 
@@ -252,7 +253,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 			goto out_unlock;
 		}
 
-        /* Calculate how many eraseblocks are requested */
+	/* Calculate how many eraseblocks are requested */
 	vol->usable_leb_size = ubi->leb_size - ubi->leb_size % req->alignment;
 	bytes = req->bytes;
 	if (do_div(bytes, vol->usable_leb_size))
@@ -274,7 +275,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	vol->data_pad  = ubi->leb_size % vol->alignment;
 	vol->vol_type  = req->vol_type;
 	vol->name_len  = req->name_len;
-	memcpy(vol->name, req->name, vol->name_len + 1);
+	memcpy(vol->name, req->name, vol->name_len);
 	vol->ubi = ubi;
 
 	/*
@@ -349,7 +350,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 		vtbl_rec.vol_type = UBI_VID_DYNAMIC;
 	else
 		vtbl_rec.vol_type = UBI_VID_STATIC;
-	memcpy(vtbl_rec.name, vol->name, vol->name_len + 1);
+	memcpy(vtbl_rec.name, vol->name, vol->name_len);
 
 	err = ubi_change_vtbl_record(ubi, vol_id, &vtbl_rec);
 	if (err)
@@ -360,19 +361,19 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	ubi->vol_count += 1;
 	spin_unlock(&ubi->volumes_lock);
 
-	paranoid_check_volumes(ubi);
-	return 0;
+	err = paranoid_check_volumes(ubi);
+	return err;
 
 out_sysfs:
 	/*
-	 * We have registered our device, we should not free the volume*
+	 * We have registered our device, we should not free the volume
 	 * description object in this function in case of an error - it is
 	 * freed by the release function.
 	 *
 	 * Get device reference to prevent the release function from being
 	 * called just after sysfs has been closed.
 	 */
-	dont_free = 1;
+	do_free = 0;
 	get_device(&vol->dev);
 	volume_sysfs_close(vol);
 out_gluebi:
@@ -382,17 +383,18 @@ out_gluebi:
 out_cdev:
 	cdev_del(&vol->cdev);
 out_mapping:
-	kfree(vol->eba_tbl);
+	if (do_free)
+		kfree(vol->eba_tbl);
 out_acc:
 	spin_lock(&ubi->volumes_lock);
 	ubi->rsvd_pebs -= vol->reserved_pebs;
 	ubi->avail_pebs += vol->reserved_pebs;
 out_unlock:
 	spin_unlock(&ubi->volumes_lock);
-	if (dont_free)
-		put_device(&vol->dev);
-	else
+	if (do_free)
 		kfree(vol);
+	else
+		put_device(&vol->dev);
 	ubi_err("cannot create volume %d, error %d", vol_id, err);
 	return err;
 }
@@ -400,19 +402,20 @@ out_unlock:
 /**
  * ubi_remove_volume - remove volume.
  * @desc: volume descriptor
+ * @no_vtbl: do not change volume table if not zero
  *
  * This function removes volume described by @desc. The volume has to be opened
  * in "exclusive" mode. Returns zero in case of success and a negative error
  * code in case of failure. The caller has to have the @ubi->volumes_mutex
  * locked.
  */
-int ubi_remove_volume(struct ubi_volume_desc *desc)
+int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl)
 {
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
 	int i, err, vol_id = vol->vol_id, reserved_pebs = vol->reserved_pebs;
 
-	dbg_msg("remove UBI volume %d", vol_id);
+	dbg_gen("remove UBI volume %d", vol_id);
 	ubi_assert(desc->mode == UBI_EXCLUSIVE);
 	ubi_assert(vol == ubi->volumes[vol_id]);
 
@@ -435,9 +438,11 @@ int ubi_remove_volume(struct ubi_volume_desc *desc)
 	if (err)
 		goto out_err;
 
-	err = ubi_change_vtbl_record(ubi, vol_id, NULL);
-	if (err)
-		goto out_err;
+	if (!no_vtbl) {
+		err = ubi_change_vtbl_record(ubi, vol_id, NULL);
+		if (err)
+			goto out_err;
+	}
 
 	for (i = 0; i < vol->reserved_pebs; i++) {
 		err = ubi_eba_unmap_leb(ubi, vol, i);
@@ -445,8 +450,6 @@ int ubi_remove_volume(struct ubi_volume_desc *desc)
 			goto out_err;
 	}
 
-	kfree(vol->eba_tbl);
-	vol->eba_tbl = NULL;
 	cdev_del(&vol->cdev);
 	volume_sysfs_close(vol);
 
@@ -465,8 +468,9 @@ int ubi_remove_volume(struct ubi_volume_desc *desc)
 	ubi->vol_count -= 1;
 	spin_unlock(&ubi->volumes_lock);
 
-	paranoid_check_volumes(ubi);
-	return 0;
+	if (!no_vtbl)
+		err = paranoid_check_volumes(ubi);
+	return err;
 
 out_err:
 	ubi_err("cannot remove volume %d, error %d", vol_id, err);
@@ -497,7 +501,7 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 	if (ubi->ro_mode)
 		return -EROFS;
 
-	dbg_msg("re-size volume %d to from %d to %d PEBs",
+	dbg_gen("re-size volume %d to from %d to %d PEBs",
 		vol_id, vol->reserved_pebs, reserved_pebs);
 
 	if (vol->vol_type == UBI_STATIC_VOLUME &&
@@ -586,8 +590,8 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 			(long long)vol->used_ebs * vol->usable_leb_size;
 	}
 
-	paranoid_check_volumes(ubi);
-	return 0;
+	err = paranoid_check_volumes(ubi);
+	return err;
 
 out_acc:
 	if (pebs > 0) {
@@ -602,6 +606,44 @@ out_free:
 }
 
 /**
+ * ubi_rename_volumes - re-name UBI volumes.
+ * @ubi: UBI device description object
+ * @rename_list: list of &struct ubi_rename_entry objects
+ *
+ * This function re-names or removes volumes specified in the re-name list.
+ * Returns zero in case of success and a negative error code in case of
+ * failure.
+ */
+int ubi_rename_volumes(struct ubi_device *ubi, struct list_head *rename_list)
+{
+	int err;
+	struct ubi_rename_entry *re;
+
+	err = ubi_vtbl_rename_volumes(ubi, rename_list);
+	if (err)
+		return err;
+
+	list_for_each_entry(re, rename_list, list) {
+		if (re->remove) {
+			err = ubi_remove_volume(re->desc, 1);
+			if (err)
+				break;
+		} else {
+			struct ubi_volume *vol = re->desc->vol;
+
+			spin_lock(&ubi->volumes_lock);
+			vol->name_len = re->new_name_len;
+			memcpy(vol->name, re->new_name, re->new_name_len + 1);
+			spin_unlock(&ubi->volumes_lock);
+		}
+	}
+
+	if (!err)
+		err = paranoid_check_volumes(ubi);
+	return err;
+}
+
+/**
  * ubi_add_volume - add volume.
  * @ubi: UBI device description object
  * @vol: volume description object
@@ -615,8 +657,7 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	int err, vol_id = vol->vol_id;
 	dev_t dev;
 
-	dbg_msg("add volume %d", vol_id);
-	ubi_dbg_dump_vol_info(vol);
+	dbg_gen("add volume %d", vol_id);
 
 	/* Register character device for the volume */
 	cdev_init(&vol->cdev, &ubi_vol_cdev_operations);
@@ -650,8 +691,8 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 		return err;
 	}
 
-	paranoid_check_volumes(ubi);
-	return 0;
+	err = paranoid_check_volumes(ubi);
+	return err;
 
 out_gluebi:
 	err = ubi_destroy_gluebi(vol);
@@ -672,7 +713,7 @@ void ubi_free_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 {
 	int err;
 
-	dbg_msg("free volume %d", vol->vol_id);
+	dbg_gen("free volume %d", vol->vol_id);
 
 	ubi->volumes[vol->vol_id] = NULL;
 	err = ubi_destroy_gluebi(vol);
@@ -686,8 +727,10 @@ void ubi_free_volume(struct ubi_device *ubi, struct ubi_volume *vol)
  * paranoid_check_volume - check volume information.
  * @ubi: UBI device description object
  * @vol_id: volume ID
+ *
+ * Returns zero if volume is all right and a a negative error code if not.
  */
-static void paranoid_check_volume(struct ubi_device *ubi, int vol_id)
+static int paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 {
 	int idx = vol_id2idx(ubi, vol_id);
 	int reserved_pebs, alignment, data_pad, vol_type, name_len, upd_marker;
@@ -705,16 +748,7 @@ static void paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 			goto fail;
 		}
 		spin_unlock(&ubi->volumes_lock);
-		return;
-	}
-
-	if (vol->exclusive) {
-		/*
-		 * The volume may be being created at the moment, do not check
-		 * it (e.g., it may be in the middle of ubi_create_volume().
-		 */
-		spin_unlock(&ubi->volumes_lock);
-		return;
+		return 0;
 	}
 
 	if (vol->reserved_pebs < 0 || vol->alignment < 0 || vol->data_pad < 0 ||
@@ -727,7 +761,7 @@ static void paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 		goto fail;
 	}
 
-	n = vol->alignment % ubi->min_io_size;
+	n = vol->alignment & (ubi->min_io_size - 1);
 	if (vol->alignment != 1 && n) {
 		ubi_err("alignment is not multiple of min I/O unit");
 		goto fail;
@@ -824,31 +858,39 @@ static void paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 
 	if (alignment != vol->alignment || data_pad != vol->data_pad ||
 	    upd_marker != vol->upd_marker || vol_type != vol->vol_type ||
-	    name_len!= vol->name_len || strncmp(name, vol->name, name_len)) {
+	    name_len != vol->name_len || strncmp(name, vol->name, name_len)) {
 		ubi_err("volume info is different");
 		goto fail;
 	}
 
 	spin_unlock(&ubi->volumes_lock);
-	return;
+	return 0;
 
 fail:
 	ubi_err("paranoid check failed for volume %d", vol_id);
-	ubi_dbg_dump_vol_info(vol);
+	if (vol)
+		ubi_dbg_dump_vol_info(vol);
 	ubi_dbg_dump_vtbl_record(&ubi->vtbl[vol_id], vol_id);
 	spin_unlock(&ubi->volumes_lock);
-	BUG();
+	return -EINVAL;
 }
 
 /**
  * paranoid_check_volumes - check information about all volumes.
  * @ubi: UBI device description object
+ *
+ * Returns zero if volumes are all right and a a negative error code if not.
  */
-static void paranoid_check_volumes(struct ubi_device *ubi)
+static int paranoid_check_volumes(struct ubi_device *ubi)
 {
-	int i;
+	int i, err = 0;
 
-	for (i = 0; i < ubi->vtbl_slots; i++)
-		paranoid_check_volume(ubi, i);
+	for (i = 0; i < ubi->vtbl_slots; i++) {
+		err = paranoid_check_volume(ubi, i);
+		if (err)
+			break;
+	}
+
+	return err;
 }
 #endif
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index af36b12be27..217d0e111b2 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -115,8 +115,58 @@ int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 }
 
 /**
- * vtbl_check - check if volume table is not corrupted and contains sensible
- *              data.
+ * ubi_vtbl_rename_volumes - rename UBI volumes in the volume table.
+ * @ubi: UBI device description object
+ * @rename_list: list of &struct ubi_rename_entry objects
+ *
+ * This function re-names multiple volumes specified in @req in the volume
+ * table. Returns zero in case of success and a negative error code in case of
+ * failure.
+ */
+int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
+			    struct list_head *rename_list)
+{
+	int i, err;
+	struct ubi_rename_entry *re;
+	struct ubi_volume *layout_vol;
+
+	list_for_each_entry(re, rename_list, list) {
+		uint32_t crc;
+		struct ubi_volume *vol = re->desc->vol;
+		struct ubi_vtbl_record *vtbl_rec = &ubi->vtbl[vol->vol_id];
+
+		if (re->remove) {
+			memcpy(vtbl_rec, &empty_vtbl_record,
+			       sizeof(struct ubi_vtbl_record));
+			continue;
+		}
+
+		vtbl_rec->name_len = cpu_to_be16(re->new_name_len);
+		memcpy(vtbl_rec->name, re->new_name, re->new_name_len);
+		memset(vtbl_rec->name + re->new_name_len, 0,
+		       UBI_VOL_NAME_MAX + 1 - re->new_name_len);
+		crc = crc32(UBI_CRC32_INIT, vtbl_rec,
+			    UBI_VTBL_RECORD_SIZE_CRC);
+		vtbl_rec->crc = cpu_to_be32(crc);
+	}
+
+	layout_vol = ubi->volumes[vol_id2idx(ubi, UBI_LAYOUT_VOLUME_ID)];
+	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
+		err = ubi_eba_unmap_leb(ubi, layout_vol, i);
+		if (err)
+			return err;
+
+		err = ubi_eba_write_leb(ubi, layout_vol, i, ubi->vtbl, 0,
+					ubi->vtbl_size, UBI_LONGTERM);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+/**
+ * vtbl_check - check if volume table is not corrupted and sensible.
  * @ubi: UBI device description object
  * @vtbl: volume table
  *
@@ -127,7 +177,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 		      const struct ubi_vtbl_record *vtbl)
 {
 	int i, n, reserved_pebs, alignment, data_pad, vol_type, name_len;
-	int upd_marker;
+	int upd_marker, err;
 	uint32_t crc;
 	const char *name;
 
@@ -153,7 +203,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 		if (reserved_pebs == 0) {
 			if (memcmp(&vtbl[i], &empty_vtbl_record,
 						UBI_VTBL_RECORD_SIZE)) {
-				dbg_err("bad empty record");
+				err = 2;
 				goto bad;
 			}
 			continue;
@@ -161,56 +211,57 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 		if (reserved_pebs < 0 || alignment < 0 || data_pad < 0 ||
 		    name_len < 0) {
-			dbg_err("negative values");
+			err = 3;
 			goto bad;
 		}
 
 		if (alignment > ubi->leb_size || alignment == 0) {
-			dbg_err("bad alignment");
+			err = 4;
 			goto bad;
 		}
 
-		n = alignment % ubi->min_io_size;
+		n = alignment & (ubi->min_io_size - 1);
 		if (alignment != 1 && n) {
-			dbg_err("alignment is not multiple of min I/O unit");
+			err = 5;
 			goto bad;
 		}
 
 		n = ubi->leb_size % alignment;
 		if (data_pad != n) {
 			dbg_err("bad data_pad, has to be %d", n);
+			err = 6;
 			goto bad;
 		}
 
 		if (vol_type != UBI_VID_DYNAMIC && vol_type != UBI_VID_STATIC) {
-			dbg_err("bad vol_type");
+			err = 7;
 			goto bad;
 		}
 
 		if (upd_marker != 0 && upd_marker != 1) {
-			dbg_err("bad upd_marker");
+			err = 8;
 			goto bad;
 		}
 
 		if (reserved_pebs > ubi->good_peb_count) {
 			dbg_err("too large reserved_pebs, good PEBs %d",
 				ubi->good_peb_count);
+			err = 9;
 			goto bad;
 		}
 
 		if (name_len > UBI_VOL_NAME_MAX) {
-			dbg_err("too long volume name, max %d",
-				UBI_VOL_NAME_MAX);
+			err = 10;
 			goto bad;
 		}
 
 		if (name[0] == '\0') {
-			dbg_err("NULL volume name");
+			err = 11;
 			goto bad;
 		}
 
 		if (name_len != strnlen(name, name_len + 1)) {
-			dbg_err("bad name_len");
+			err = 12;
 			goto bad;
 		}
 	}
@@ -235,7 +286,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 	return 0;
 
 bad:
-	ubi_err("volume table check failed, record %d", i);
+	ubi_err("volume table check failed: record %d, error %d", i, err);
 	ubi_dbg_dump_vtbl_record(&vtbl[i], i);
 	return -EINVAL;
 }
@@ -287,7 +338,6 @@ retry:
 			     vid_hdr->data_pad = cpu_to_be32(0);
 	vid_hdr->lnum = cpu_to_be32(copy);
 	vid_hdr->sqnum = cpu_to_be64(++si->max_sqnum);
-	vid_hdr->leb_ver = cpu_to_be32(old_seb ? old_seb->leb_ver + 1: 0);
 
 	/* The EC header is already there, write the VID header */
 	err = ubi_io_write_vid_hdr(ubi, new_seb->pnum, vid_hdr);
@@ -370,7 +420,7 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 	 *    to LEB 0.
 	 */
 
-	dbg_msg("check layout volume");
+	dbg_gen("check layout volume");
 
 	/* Read both LEB 0 and LEB 1 into memory */
 	ubi_rb_for_each_entry(rb, seb, &sv->root, u.rb) {
@@ -384,7 +434,16 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 		err = ubi_io_read_data(ubi, leb[seb->lnum], seb->pnum, 0,
 				       ubi->vtbl_size);
 		if (err == UBI_IO_BITFLIPS || err == -EBADMSG)
-			/* Scrub the PEB later */
+			/*
+			 * Scrub the PEB later. Note, -EBADMSG indicates an
+			 * uncorrectable ECC error, but we have our own CRC and
+			 * the data will be checked later. If the data is OK,
+			 * the PEB will be scrubbed (because we set
+			 * seb->scrub). If the data is not OK, the contents of
+			 * the PEB will be recovered from the second copy, and
+			 * seb->scrub will be cleared in
+			 * 'ubi_scan_add_used()'.
+			 */
 			seb->scrub = 1;
 		else if (err)
 			goto out_free;
@@ -400,7 +459,8 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 	if (!leb_corrupted[0]) {
 		/* LEB 0 is OK */
 		if (leb[1])
-			leb_corrupted[1] = memcmp(leb[0], leb[1], ubi->vtbl_size);
+			leb_corrupted[1] = memcmp(leb[0], leb[1],
+						  ubi->vtbl_size);
 		if (leb_corrupted[1]) {
 			ubi_warn("volume table copy #2 is corrupted");
 			err = create_vtbl(ubi, si, 1, leb[0]);
@@ -620,30 +680,32 @@ static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
 static int check_sv(const struct ubi_volume *vol,
 		    const struct ubi_scan_volume *sv)
 {
+	int err;
+
 	if (sv->highest_lnum >= vol->reserved_pebs) {
-		dbg_err("bad highest_lnum");
+		err = 1;
 		goto bad;
 	}
 	if (sv->leb_count > vol->reserved_pebs) {
-		dbg_err("bad leb_count");
+		err = 2;
 		goto bad;
 	}
 	if (sv->vol_type != vol->vol_type) {
-		dbg_err("bad vol_type");
+		err = 3;
 		goto bad;
 	}
 	if (sv->used_ebs > vol->reserved_pebs) {
-		dbg_err("bad used_ebs");
+		err = 4;
 		goto bad;
 	}
 	if (sv->data_pad != vol->data_pad) {
-		dbg_err("bad data_pad");
+		err = 5;
 		goto bad;
 	}
 	return 0;
 
 bad:
-	ubi_err("bad scanning information");
+	ubi_err("bad scanning information, error %d", err);
 	ubi_dbg_dump_sv(sv);
 	ubi_dbg_dump_vol_info(vol);
 	return -EINVAL;
@@ -672,14 +734,13 @@ static int check_scanning_info(const struct ubi_device *ubi,
 		return -EINVAL;
 	}
 
-	if (si->highest_vol_id >= ubi->vtbl_slots + UBI_INT_VOL_COUNT&&
+	if (si->highest_vol_id >= ubi->vtbl_slots + UBI_INT_VOL_COUNT &&
 	    si->highest_vol_id < UBI_INTERNAL_VOL_START) {
 		ubi_err("too large volume ID %d found by scanning",
 			si->highest_vol_id);
 		return -EINVAL;
 	}
 
-
 	for (i = 0; i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++) {
 		cond_resched();
 
@@ -717,8 +778,7 @@ static int check_scanning_info(const struct ubi_device *ubi,
 }
 
 /**
- * ubi_read_volume_table - read volume table.
- * information.
+ * ubi_read_volume_table - read the volume table.
  * @ubi: UBI device description object
  * @si: scanning information
  *
@@ -797,11 +857,10 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
 
 out_free:
 	vfree(ubi->vtbl);
-	for (i = 0; i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++)
-		if (ubi->volumes[i]) {
-			kfree(ubi->volumes[i]);
-			ubi->volumes[i] = NULL;
-		}
+	for (i = 0; i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++) {
+		kfree(ubi->volumes[i]);
+		ubi->volumes[i] = NULL;
+	}
 	return err;
 }
 
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index a471a491f0a..05d70937b54 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -19,22 +19,22 @@
  */
 
 /*
- * UBI wear-leveling unit.
+ * UBI wear-leveling sub-system.
  *
- * This unit is responsible for wear-leveling. It works in terms of physical
- * eraseblocks and erase counters and knows nothing about logical eraseblocks,
- * volumes, etc. From this unit's perspective all physical eraseblocks are of
- * two types - used and free. Used physical eraseblocks are those that were
- * "get" by the 'ubi_wl_get_peb()' function, and free physical eraseblocks are
- * those that were put by the 'ubi_wl_put_peb()' function.
+ * This sub-system is responsible for wear-leveling. It works in terms of
+ * physical* eraseblocks and erase counters and knows nothing about logical
+ * eraseblocks, volumes, etc. From this sub-system's perspective all physical
+ * eraseblocks are of two types - used and free. Used physical eraseblocks are
+ * those that were "get" by the 'ubi_wl_get_peb()' function, and free physical
+ * eraseblocks are those that were put by the 'ubi_wl_put_peb()' function.
  *
  * Physical eraseblocks returned by 'ubi_wl_get_peb()' have only erase counter
- * header. The rest of the physical eraseblock contains only 0xFF bytes.
+ * header. The rest of the physical eraseblock contains only %0xFF bytes.
  *
- * When physical eraseblocks are returned to the WL unit by means of the
+ * When physical eraseblocks are returned to the WL sub-system by means of the
  * 'ubi_wl_put_peb()' function, they are scheduled for erasure. The erasure is
  * done asynchronously in context of the per-UBI device background thread,
- * which is also managed by the WL unit.
+ * which is also managed by the WL sub-system.
  *
  * The wear-leveling is ensured by means of moving the contents of used
  * physical eraseblocks with low erase counter to free physical eraseblocks
@@ -43,34 +43,36 @@
  * The 'ubi_wl_get_peb()' function accepts data type hints which help to pick
  * an "optimal" physical eraseblock. For example, when it is known that the
  * physical eraseblock will be "put" soon because it contains short-term data,
- * the WL unit may pick a free physical eraseblock with low erase counter, and
- * so forth.
+ * the WL sub-system may pick a free physical eraseblock with low erase
+ * counter, and so forth.
  *
- * If the WL unit fails to erase a physical eraseblock, it marks it as bad.
+ * If the WL sub-system fails to erase a physical eraseblock, it marks it as
+ * bad.
  *
- * This unit is also responsible for scrubbing. If a bit-flip is detected in a
- * physical eraseblock, it has to be moved. Technically this is the same as
- * moving it for wear-leveling reasons.
+ * This sub-system is also responsible for scrubbing. If a bit-flip is detected
+ * in a physical eraseblock, it has to be moved. Technically this is the same
+ * as moving it for wear-leveling reasons.
  *
- * As it was said, for the UBI unit all physical eraseblocks are either "free"
- * or "used". Free eraseblock are kept in the @wl->free RB-tree, while used
- * eraseblocks are kept in a set of different RB-trees: @wl->used,
+ * As it was said, for the UBI sub-system all physical eraseblocks are either
+ * "free" or "used". Free eraseblock are kept in the @wl->free RB-tree, while
+ * used eraseblocks are kept in a set of different RB-trees: @wl->used,
  * @wl->prot.pnum, @wl->prot.aec, and @wl->scrub.
  *
  * Note, in this implementation, we keep a small in-RAM object for each physical
  * eraseblock. This is surely not a scalable solution. But it appears to be good
  * enough for moderately large flashes and it is simple. In future, one may
- * re-work this unit and make it more scalable.
+ * re-work this sub-system and make it more scalable.
  *
- * At the moment this unit does not utilize the sequence number, which was
- * introduced relatively recently. But it would be wise to do this because the
- * sequence number of a logical eraseblock characterizes how old is it. For
+ * At the moment this sub-system does not utilize the sequence number, which
+ * was introduced relatively recently. But it would be wise to do this because
+ * the sequence number of a logical eraseblock characterizes how old is it. For
  * example, when we move a PEB with low erase counter, and we need to pick the
  * target PEB, we pick a PEB with the highest EC if our PEB is "old" and we
  * pick target PEB with an average EC if our PEB is not very "old". This is a
- * room for future re-works of the WL unit.
+ * room for future re-works of the WL sub-system.
  *
- * FIXME: looks too complex, should be simplified (later).
+ * Note: the stuff with protection trees looks too complex and is difficult to
+ * understand. Should be fixed.
  */
 
 #include <linux/slab.h>
@@ -92,20 +94,21 @@
 
 /*
  * Maximum difference between two erase counters. If this threshold is
- * exceeded, the WL unit starts moving data from used physical eraseblocks with
- * low erase counter to free physical eraseblocks with high erase counter.
+ * exceeded, the WL sub-system starts moving data from used physical
+ * eraseblocks with low erase counter to free physical eraseblocks with high
+ * erase counter.
  */
 #define UBI_WL_THRESHOLD CONFIG_MTD_UBI_WL_THRESHOLD
 
 /*
- * When a physical eraseblock is moved, the WL unit has to pick the target
+ * When a physical eraseblock is moved, the WL sub-system has to pick the target
  * physical eraseblock to move to. The simplest way would be just to pick the
  * one with the highest erase counter. But in certain workloads this could lead
  * to an unlimited wear of one or few physical eraseblock. Indeed, imagine a
  * situation when the picked physical eraseblock is constantly erased after the
  * data is written to it. So, we have a constant which limits the highest erase
- * counter of the free physical eraseblock to pick. Namely, the WL unit does
- * not pick eraseblocks with erase counter greater then the lowest erase
+ * counter of the free physical eraseblock to pick. Namely, the WL sub-system
+ * does not pick eraseblocks with erase counter greater then the lowest erase
  * counter plus %WL_FREE_MAX_DIFF.
  */
 #define WL_FREE_MAX_DIFF (2*UBI_WL_THRESHOLD)
@@ -123,11 +126,11 @@
  * @abs_ec: the absolute erase counter value when the protection ends
  * @e: the wear-leveling entry of the physical eraseblock under protection
  *
- * When the WL unit returns a physical eraseblock, the physical eraseblock is
- * protected from being moved for some "time". For this reason, the physical
- * eraseblock is not directly moved from the @wl->free tree to the @wl->used
- * tree. There is one more tree in between where this physical eraseblock is
- * temporarily stored (@wl->prot).
+ * When the WL sub-system returns a physical eraseblock, the physical
+ * eraseblock is protected from being moved for some "time". For this reason,
+ * the physical eraseblock is not directly moved from the @wl->free tree to the
+ * @wl->used tree. There is one more tree in between where this physical
+ * eraseblock is temporarily stored (@wl->prot).
  *
  * All this protection stuff is needed because:
  *  o we don't want to move physical eraseblocks just after we have given them
@@ -175,7 +178,6 @@ struct ubi_wl_prot_entry {
  * @list: a link in the list of pending works
  * @func: worker function
  * @priv: private data of the worker function
- *
  * @e: physical eraseblock to erase
  * @torture: if the physical eraseblock has to be tortured
  *
@@ -473,52 +475,47 @@ retry:
 	}
 
 	switch (dtype) {
-		case UBI_LONGTERM:
-			/*
-			 * For long term data we pick a physical eraseblock
-			 * with high erase counter. But the highest erase
-			 * counter we can pick is bounded by the the lowest
-			 * erase counter plus %WL_FREE_MAX_DIFF.
-			 */
-			e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
-			protect = LT_PROTECTION;
-			break;
-		case UBI_UNKNOWN:
-			/*
-			 * For unknown data we pick a physical eraseblock with
-			 * medium erase counter. But we by no means can pick a
-			 * physical eraseblock with erase counter greater or
-			 * equivalent than the lowest erase counter plus
-			 * %WL_FREE_MAX_DIFF.
-			 */
-			first = rb_entry(rb_first(&ubi->free),
-					 struct ubi_wl_entry, rb);
-			last = rb_entry(rb_last(&ubi->free),
-					struct ubi_wl_entry, rb);
+	case UBI_LONGTERM:
+		/*
+		 * For long term data we pick a physical eraseblock with high
+		 * erase counter. But the highest erase counter we can pick is
+		 * bounded by the the lowest erase counter plus
+		 * %WL_FREE_MAX_DIFF.
+		 */
+		e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
+		protect = LT_PROTECTION;
+		break;
+	case UBI_UNKNOWN:
+		/*
+		 * For unknown data we pick a physical eraseblock with medium
+		 * erase counter. But we by no means can pick a physical
+		 * eraseblock with erase counter greater or equivalent than the
+		 * lowest erase counter plus %WL_FREE_MAX_DIFF.
+		 */
+		first = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry, rb);
+		last = rb_entry(rb_last(&ubi->free), struct ubi_wl_entry, rb);
 
-			if (last->ec - first->ec < WL_FREE_MAX_DIFF)
-				e = rb_entry(ubi->free.rb_node,
-						struct ubi_wl_entry, rb);
-			else {
-				medium_ec = (first->ec + WL_FREE_MAX_DIFF)/2;
-				e = find_wl_entry(&ubi->free, medium_ec);
-			}
-			protect = U_PROTECTION;
-			break;
-		case UBI_SHORTTERM:
-			/*
-			 * For short term data we pick a physical eraseblock
-			 * with the lowest erase counter as we expect it will
-			 * be erased soon.
-			 */
-			e = rb_entry(rb_first(&ubi->free),
-				     struct ubi_wl_entry, rb);
-			protect = ST_PROTECTION;
-			break;
-		default:
-			protect = 0;
-			e = NULL;
-			BUG();
+		if (last->ec - first->ec < WL_FREE_MAX_DIFF)
+			e = rb_entry(ubi->free.rb_node,
+					struct ubi_wl_entry, rb);
+		else {
+			medium_ec = (first->ec + WL_FREE_MAX_DIFF)/2;
+			e = find_wl_entry(&ubi->free, medium_ec);
+		}
+		protect = U_PROTECTION;
+		break;
+	case UBI_SHORTTERM:
+		/*
+		 * For short term data we pick a physical eraseblock with the
+		 * lowest erase counter as we expect it will be erased soon.
+		 */
+		e = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry, rb);
+		protect = ST_PROTECTION;
+		break;
+	default:
+		protect = 0;
+		e = NULL;
+		BUG();
 	}
 
 	/*
@@ -582,7 +579,8 @@ found:
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-static int sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e, int torture)
+static int sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
+		      int torture)
 {
 	int err;
 	struct ubi_ec_hdr *ec_hdr;
@@ -634,8 +632,7 @@ out_free:
 }
 
 /**
- * check_protection_over - check if it is time to stop protecting some
- * physical eraseblocks.
+ * check_protection_over - check if it is time to stop protecting some PEBs.
  * @ubi: UBI device description object
  *
  * This function is called after each erase operation, when the absolute erase
@@ -871,6 +868,10 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	}
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
+	if (scrubbing && !protect)
+		ubi_msg("scrubbed PEB %d, data moved to PEB %d",
+			e1->pnum, e2->pnum);
+
 	spin_lock(&ubi->wl_lock);
 	if (protect)
 		prot_tree_add(ubi, e1, pe, protect);
@@ -1054,8 +1055,8 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		spin_unlock(&ubi->wl_lock);
 
 		/*
-		 * One more erase operation has happened, take care about protected
-		 * physical eraseblocks.
+		 * One more erase operation has happened, take care about
+		 * protected physical eraseblocks.
 		 */
 		check_protection_over(ubi);
 
@@ -1136,7 +1137,7 @@ out_ro:
 }
 
 /**
- * ubi_wl_put_peb - return a physical eraseblock to the wear-leveling unit.
+ * ubi_wl_put_peb - return a PEB to the wear-leveling sub-system.
  * @ubi: UBI device description object
  * @pnum: physical eraseblock to return
  * @torture: if this physical eraseblock has to be tortured
@@ -1175,11 +1176,11 @@ retry:
 		/*
 		 * User is putting the physical eraseblock which was selected
 		 * as the target the data is moved to. It may happen if the EBA
-		 * unit already re-mapped the LEB in 'ubi_eba_copy_leb()' but
-		 * the WL unit has not put the PEB to the "used" tree yet, but
-		 * it is about to do this. So we just set a flag which will
-		 * tell the WL worker that the PEB is not needed anymore and
-		 * should be scheduled for erasure.
+		 * sub-system already re-mapped the LEB in 'ubi_eba_copy_leb()'
+		 * but the WL sub-system has not put the PEB to the "used" tree
+		 * yet, but it is about to do this. So we just set a flag which
+		 * will tell the WL worker that the PEB is not needed anymore
+		 * and should be scheduled for erasure.
 		 */
 		dbg_wl("PEB %d is the target of data moving", pnum);
 		ubi_assert(!ubi->move_to_put);
@@ -1229,7 +1230,7 @@ int ubi_wl_scrub_peb(struct ubi_device *ubi, int pnum)
 {
 	struct ubi_wl_entry *e;
 
-	ubi_msg("schedule PEB %d for scrubbing", pnum);
+	dbg_msg("schedule PEB %d for scrubbing", pnum);
 
 retry:
 	spin_lock(&ubi->wl_lock);
@@ -1368,7 +1369,7 @@ int ubi_thread(void *u)
 		int err;
 
 		if (kthread_should_stop())
-			goto out;
+			break;
 
 		if (try_to_freeze())
 			continue;
@@ -1403,7 +1404,6 @@ int ubi_thread(void *u)
 		cond_resched();
 	}
 
-out:
 	dbg_wl("background thread \"%s\" is killed", ubi->bgt_name);
 	return 0;
 }
@@ -1426,8 +1426,7 @@ static void cancel_pending(struct ubi_device *ubi)
 }
 
 /**
- * ubi_wl_init_scan - initialize the wear-leveling unit using scanning
- * information.
+ * ubi_wl_init_scan - initialize the WL sub-system using scanning information.
  * @ubi: UBI device description object
  * @si: scanning information
  *
@@ -1584,13 +1583,12 @@ static void protection_trees_destroy(struct ubi_device *ubi)
 }
 
 /**
- * ubi_wl_close - close the wear-leveling unit.
+ * ubi_wl_close - close the wear-leveling sub-system.
  * @ubi: UBI device description object
  */
 void ubi_wl_close(struct ubi_device *ubi)
 {
-	dbg_wl("close the UBI wear-leveling unit");
-
+	dbg_wl("close the WL sub-system");
 	cancel_pending(ubi);
 	protection_trees_destroy(ubi);
 	tree_destroy(&ubi->used);
@@ -1602,8 +1600,7 @@ void ubi_wl_close(struct ubi_device *ubi)
 #ifdef CONFIG_MTD_UBI_DEBUG_PARANOID
 
 /**
- * paranoid_check_ec - make sure that the erase counter of a physical eraseblock
- * is correct.
+ * paranoid_check_ec - make sure that the erase counter of a PEB is correct.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  * @ec: the erase counter to check
@@ -1644,13 +1641,12 @@ out_free:
 }
 
 /**
- * paranoid_check_in_wl_tree - make sure that a wear-leveling entry is present
- * in a WL RB-tree.
+ * paranoid_check_in_wl_tree - check that wear-leveling entry is in WL RB-tree.
  * @e: the wear-leveling entry to check
  * @root: the root of the tree
  *
- * This function returns zero if @e is in the @root RB-tree and %1 if it
- * is not.
+ * This function returns zero if @e is in the @root RB-tree and %1 if it is
+ * not.
  */
 static int paranoid_check_in_wl_tree(struct ubi_wl_entry *e,
 				     struct rb_root *root)
diff --git a/drivers/net/arm/ep93xx_eth.c b/drivers/net/arm/ep93xx_eth.c
index 7a14980f347..18d3eeb7eab 100644
--- a/drivers/net/arm/ep93xx_eth.c
+++ b/drivers/net/arm/ep93xx_eth.c
@@ -482,7 +482,7 @@ static int ep93xx_alloc_buffers(struct ep93xx_priv *ep)
 			goto err;
 
 		d = dma_map_single(NULL, page, PAGE_SIZE, DMA_FROM_DEVICE);
-		if (dma_mapping_error(d)) {
+		if (dma_mapping_error(NULL, d)) {
 			free_page((unsigned long)page);
 			goto err;
 		}
@@ -505,7 +505,7 @@ static int ep93xx_alloc_buffers(struct ep93xx_priv *ep)
 			goto err;
 
 		d = dma_map_single(NULL, page, PAGE_SIZE, DMA_TO_DEVICE);
-		if (dma_mapping_error(d)) {
+		if (dma_mapping_error(NULL, d)) {
 			free_page((unsigned long)page);
 			goto err;
 		}
diff --git a/drivers/net/bnx2x_main.c b/drivers/net/bnx2x_main.c
index 0263bef9cc6..af251a5df84 100644
--- a/drivers/net/bnx2x_main.c
+++ b/drivers/net/bnx2x_main.c
@@ -814,7 +814,7 @@ static u16 bnx2x_free_tx_pkt(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	}
 
 	/* release skb */
-	BUG_TRAP(skb);
+	WARN_ON(!skb);
 	dev_kfree_skb(skb);
 	tx_buf->first_bd = 0;
 	tx_buf->skb = NULL;
@@ -837,9 +837,9 @@ static inline u16 bnx2x_tx_avail(struct bnx2x_fastpath *fp)
 	used = SUB_S16(prod, cons) + (s16)NUM_TX_RINGS;
 
 #ifdef BNX2X_STOP_ON_ERROR
-	BUG_TRAP(used >= 0);
-	BUG_TRAP(used <= fp->bp->tx_ring_size);
-	BUG_TRAP((fp->bp->tx_ring_size - used) <= MAX_TX_AVAIL);
+	WARN_ON(used < 0);
+	WARN_ON(used > fp->bp->tx_ring_size);
+	WARN_ON((fp->bp->tx_ring_size - used) > MAX_TX_AVAIL);
 #endif
 
 	return (s16)(fp->bp->tx_ring_size) - used;
@@ -1020,7 +1020,7 @@ static inline int bnx2x_alloc_rx_sge(struct bnx2x *bp,
 
 	mapping = pci_map_page(bp->pdev, page, 0, BCM_PAGE_SIZE*PAGES_PER_SGE,
 			       PCI_DMA_FROMDEVICE);
-	if (unlikely(dma_mapping_error(mapping))) {
+	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
 		__free_pages(page, PAGES_PER_SGE_SHIFT);
 		return -ENOMEM;
 	}
@@ -1048,7 +1048,7 @@ static inline int bnx2x_alloc_rx_skb(struct bnx2x *bp,
 
 	mapping = pci_map_single(bp->pdev, skb->data, bp->rx_buf_use_size,
 				 PCI_DMA_FROMDEVICE);
-	if (unlikely(dma_mapping_error(mapping))) {
+	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
 		dev_kfree_skb(skb);
 		return -ENOMEM;
 	}
@@ -4374,7 +4374,7 @@ static void bnx2x_init_rx_rings(struct bnx2x *bp)
 			}
 			ring_prod = NEXT_RX_IDX(ring_prod);
 			cqe_ring_prod = NEXT_RCQ_IDX(cqe_ring_prod);
-			BUG_TRAP(ring_prod > i);
+			WARN_ON(ring_prod <= i);
 		}
 
 		fp->rx_bd_prod = ring_prod;
diff --git a/drivers/net/cassini.c b/drivers/net/cassini.c
index 83768df2780..f1936d51b45 100644
--- a/drivers/net/cassini.c
+++ b/drivers/net/cassini.c
@@ -576,6 +576,18 @@ static void cas_spare_recover(struct cas *cp, const gfp_t flags)
 	list_for_each_safe(elem, tmp, &list) {
 		cas_page_t *page = list_entry(elem, cas_page_t, list);
 
+		/*
+		 * With the lockless pagecache, cassini buffering scheme gets
+		 * slightly less accurate: we might find that a page has an
+		 * elevated reference count here, due to a speculative ref,
+		 * and skip it as in-use. Ideally we would be able to reclaim
+		 * it. However this would be such a rare case, it doesn't
+		 * matter too much as we should pick it up the next time round.
+		 *
+		 * Importantly, if we find that the page has a refcount of 1
+		 * here (our refcount), then we know it is definitely not inuse
+		 * so we can reuse it.
+		 */
 		if (page_count(page->buffer) > 1)
 			continue;
 
diff --git a/drivers/net/cxgb3/sge.c b/drivers/net/cxgb3/sge.c
index a96331c875e..1b0861d73ab 100644
--- a/drivers/net/cxgb3/sge.c
+++ b/drivers/net/cxgb3/sge.c
@@ -386,7 +386,7 @@ static inline int add_one_rx_buf(void *va, unsigned int len,
 	dma_addr_t mapping;
 
 	mapping = pci_map_single(pdev, va, len, PCI_DMA_FROMDEVICE);
-	if (unlikely(pci_dma_mapping_error(mapping)))
+	if (unlikely(pci_dma_mapping_error(pdev, mapping)))
 		return -ENOMEM;
 
 	pci_unmap_addr_set(sd, dma_addr, mapping);
diff --git a/drivers/net/e100.c b/drivers/net/e100.c
index 1037b133231..19d32a227be 100644
--- a/drivers/net/e100.c
+++ b/drivers/net/e100.c
@@ -1790,7 +1790,7 @@ static int e100_rx_alloc_skb(struct nic *nic, struct rx *rx)
 	rx->dma_addr = pci_map_single(nic->pdev, rx->skb->data,
 		RFD_BUF_LEN, PCI_DMA_BIDIRECTIONAL);
 
-	if (pci_dma_mapping_error(rx->dma_addr)) {
+	if (pci_dma_mapping_error(nic->pdev, rx->dma_addr)) {
 		dev_kfree_skb_any(rx->skb);
 		rx->skb = NULL;
 		rx->dma_addr = 0;
diff --git a/drivers/net/e1000e/ethtool.c b/drivers/net/e1000e/ethtool.c
index a14561f40db..9350564065e 100644
--- a/drivers/net/e1000e/ethtool.c
+++ b/drivers/net/e1000e/ethtool.c
@@ -1090,7 +1090,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		tx_ring->buffer_info[i].dma =
 			pci_map_single(pdev, skb->data, skb->len,
 				       PCI_DMA_TODEVICE);
-		if (pci_dma_mapping_error(tx_ring->buffer_info[i].dma)) {
+		if (pci_dma_mapping_error(pdev, tx_ring->buffer_info[i].dma)) {
 			ret_val = 4;
 			goto err_nomem;
 		}
@@ -1153,7 +1153,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		rx_ring->buffer_info[i].dma =
 			pci_map_single(pdev, skb->data, 2048,
 				       PCI_DMA_FROMDEVICE);
-		if (pci_dma_mapping_error(rx_ring->buffer_info[i].dma)) {
+		if (pci_dma_mapping_error(pdev, rx_ring->buffer_info[i].dma)) {
 			ret_val = 8;
 			goto err_nomem;
 		}
diff --git a/drivers/net/e1000e/netdev.c b/drivers/net/e1000e/netdev.c
index 869544b8c05..d1367789976 100644
--- a/drivers/net/e1000e/netdev.c
+++ b/drivers/net/e1000e/netdev.c
@@ -195,7 +195,7 @@ map_skb:
 		buffer_info->dma = pci_map_single(pdev, skb->data,
 						  adapter->rx_buffer_len,
 						  PCI_DMA_FROMDEVICE);
-		if (pci_dma_mapping_error(buffer_info->dma)) {
+		if (pci_dma_mapping_error(pdev, buffer_info->dma)) {
 			dev_err(&pdev->dev, "RX DMA map failed\n");
 			adapter->rx_dma_failed++;
 			break;
@@ -265,7 +265,7 @@ static void e1000_alloc_rx_buffers_ps(struct e1000_adapter *adapter,
 						   ps_page->page,
 						   0, PAGE_SIZE,
 						   PCI_DMA_FROMDEVICE);
-				if (pci_dma_mapping_error(ps_page->dma)) {
+				if (pci_dma_mapping_error(pdev, ps_page->dma)) {
 					dev_err(&adapter->pdev->dev,
 					  "RX DMA page map failed\n");
 					adapter->rx_dma_failed++;
@@ -300,7 +300,7 @@ static void e1000_alloc_rx_buffers_ps(struct e1000_adapter *adapter,
 		buffer_info->dma = pci_map_single(pdev, skb->data,
 						  adapter->rx_ps_bsize0,
 						  PCI_DMA_FROMDEVICE);
-		if (pci_dma_mapping_error(buffer_info->dma)) {
+		if (pci_dma_mapping_error(pdev, buffer_info->dma)) {
 			dev_err(&pdev->dev, "RX DMA map failed\n");
 			adapter->rx_dma_failed++;
 			/* cleanup skb */
@@ -3344,7 +3344,7 @@ static int e1000_tx_map(struct e1000_adapter *adapter,
 				skb->data + offset,
 				size,
 				PCI_DMA_TODEVICE);
-		if (pci_dma_mapping_error(buffer_info->dma)) {
+		if (pci_dma_mapping_error(adapter->pdev, buffer_info->dma)) {
 			dev_err(&adapter->pdev->dev, "TX DMA map failed\n");
 			adapter->tx_dma_failed++;
 			return -1;
@@ -3382,7 +3382,8 @@ static int e1000_tx_map(struct e1000_adapter *adapter,
 					offset,
 					size,
 					PCI_DMA_TODEVICE);
-			if (pci_dma_mapping_error(buffer_info->dma)) {
+			if (pci_dma_mapping_error(adapter->pdev,
+						  buffer_info->dma)) {
 				dev_err(&adapter->pdev->dev,
 					"TX DMA page map failed\n");
 				adapter->tx_dma_failed++;
@@ -4067,8 +4068,6 @@ static void e1000_netpoll(struct net_device *netdev)
 	disable_irq(adapter->pdev->irq);
 	e1000_intr(adapter->pdev->irq, netdev);
 
-	e1000_clean_tx_irq(adapter);
-
 	enable_irq(adapter->pdev->irq);
 }
 #endif
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 32a4f17d35f..ecd5c71a7a8 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -2,12 +2,6 @@
  * Fast Ethernet Controller (FEC) driver for Motorola MPC8xx.
  * Copyright (c) 1997 Dan Malek (dmalek@jlc.net)
  *
- * This version of the driver is specific to the FADS implementation,
- * since the board contains control registers external to the processor
- * for the control of the LevelOne LXT970 transceiver.  The MPC860T manual
- * describes connections using the internal parallel port I/O, which
- * is basically all of Port D.
- *
  * Right now, I am very wasteful with the buffers.  I allocate memory
  * pages and then divide them into 2K frame buffers.  This way I know I
  * have buffers large enough to hold one frame within one buffer descriptor.
@@ -49,17 +43,9 @@
 #include <asm/pgtable.h>
 #include <asm/cacheflush.h>
 
-#if defined(CONFIG_M523x) || defined(CONFIG_M527x) || \
-    defined(CONFIG_M5272) || defined(CONFIG_M528x) || \
-    defined(CONFIG_M520x) || defined(CONFIG_M532x)
 #include <asm/coldfire.h>
 #include <asm/mcfsim.h>
 #include "fec.h"
-#else
-#include <asm/8xx_immap.h>
-#include <asm/mpc8xx.h>
-#include "commproc.h"
-#endif
 
 #if defined(CONFIG_FEC2)
 #define	FEC_MAX_PORTS	2
@@ -67,7 +53,7 @@
 #define	FEC_MAX_PORTS	1
 #endif
 
-#if defined(CONFIG_FADS) || defined(CONFIG_RPXCLASSIC) || defined(CONFIG_M5272)
+#if defined(CONFIG_M5272)
 #define HAVE_mii_link_interrupt
 #endif
 
@@ -1235,14 +1221,9 @@ static phy_info_t const * const phy_info[] = {
 
 /* ------------------------------------------------------------------------- */
 #ifdef HAVE_mii_link_interrupt
-#ifdef CONFIG_RPXCLASSIC
-static void
-mii_link_interrupt(void *dev_id);
-#else
 static irqreturn_t
 mii_link_interrupt(int irq, void * dev_id);
 #endif
-#endif
 
 #if defined(CONFIG_M5272)
 /*
@@ -1795,24 +1776,6 @@ static void __inline__ fec_request_intrs(struct net_device *dev)
 
 	if (request_8xxirq(FEC_INTERRUPT, fec_enet_interrupt, 0, "fec", dev) != 0)
 		panic("Could not allocate FEC IRQ!");
-
-#ifdef CONFIG_RPXCLASSIC
-	/* Make Port C, bit 15 an input that causes interrupts.
-	*/
-	immap->im_ioport.iop_pcpar &= ~0x0001;
-	immap->im_ioport.iop_pcdir &= ~0x0001;
-	immap->im_ioport.iop_pcso &= ~0x0001;
-	immap->im_ioport.iop_pcint |= 0x0001;
-	cpm_install_handler(CPMVEC_PIO_PC15, mii_link_interrupt, dev);
-
-	/* Make LEDS reflect Link status.
-	*/
-	*((uint *) RPX_CSR_ADDR) &= ~BCSR2_FETHLEDMODE;
-#endif
-#ifdef CONFIG_FADS
-	if (request_8xxirq(SIU_IRQ2, mii_link_interrupt, 0, "mii", dev) != 0)
-		panic("Could not allocate MII IRQ!");
-#endif
 }
 
 static void __inline__ fec_get_mac(struct net_device *dev)
@@ -1821,16 +1784,6 @@ static void __inline__ fec_get_mac(struct net_device *dev)
 
 	bd = (bd_t *)__res;
 	memcpy(dev->dev_addr, bd->bi_enetaddr, ETH_ALEN);
-
-#ifdef CONFIG_RPXCLASSIC
-	/* The Embedded Planet boards have only one MAC address in
-	 * the EEPROM, but can have two Ethernet ports.  For the
-	 * FEC port, we create another address by setting one of
-	 * the address bits above something that would have (up to
-	 * now) been allocated.
-	 */
-	dev->dev_adrd[3] |= 0x80;
-#endif
 }
 
 static void __inline__ fec_set_mii(struct net_device *dev, struct fec_enet_private *fep)
@@ -2109,13 +2062,8 @@ mii_discover_phy(uint mii_reg, struct net_device *dev)
 /* This interrupt occurs when the PHY detects a link change.
 */
 #ifdef HAVE_mii_link_interrupt
-#ifdef CONFIG_RPXCLASSIC
-static void
-mii_link_interrupt(void *dev_id)
-#else
 static irqreturn_t
 mii_link_interrupt(int irq, void * dev_id)
-#endif
 {
 	struct	net_device *dev = dev_id;
 	struct fec_enet_private *fep = netdev_priv(dev);
diff --git a/drivers/net/ibmveth.c b/drivers/net/ibmveth.c
index 00527805e4f..91ec9fdc718 100644
--- a/drivers/net/ibmveth.c
+++ b/drivers/net/ibmveth.c
@@ -33,6 +33,7 @@
 */
 
 #include <linux/module.h>
+#include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/ioport.h>
@@ -52,7 +53,9 @@
 #include <asm/hvcall.h>
 #include <asm/atomic.h>
 #include <asm/vio.h>
+#include <asm/iommu.h>
 #include <asm/uaccess.h>
+#include <asm/firmware.h>
 #include <linux/seq_file.h>
 
 #include "ibmveth.h"
@@ -94,8 +97,10 @@ static void ibmveth_proc_register_adapter(struct ibmveth_adapter *adapter);
 static void ibmveth_proc_unregister_adapter(struct ibmveth_adapter *adapter);
 static irqreturn_t ibmveth_interrupt(int irq, void *dev_instance);
 static void ibmveth_rxq_harvest_buffer(struct ibmveth_adapter *adapter);
+static unsigned long ibmveth_get_desired_dma(struct vio_dev *vdev);
 static struct kobj_type ktype_veth_pool;
 
+
 #ifdef CONFIG_PROC_FS
 #define IBMVETH_PROC_DIR "ibmveth"
 static struct proc_dir_entry *ibmveth_proc_dir;
@@ -226,16 +231,16 @@ static void ibmveth_replenish_buffer_pool(struct ibmveth_adapter *adapter, struc
 	u32 i;
 	u32 count = pool->size - atomic_read(&pool->available);
 	u32 buffers_added = 0;
+	struct sk_buff *skb;
+	unsigned int free_index, index;
+	u64 correlator;
+	unsigned long lpar_rc;
+	dma_addr_t dma_addr;
 
 	mb();
 
 	for(i = 0; i < count; ++i) {
-		struct sk_buff *skb;
-		unsigned int free_index, index;
-		u64 correlator;
 		union ibmveth_buf_desc desc;
-		unsigned long lpar_rc;
-		dma_addr_t dma_addr;
 
 		skb = alloc_skb(pool->buff_size, GFP_ATOMIC);
 
@@ -255,6 +260,9 @@ static void ibmveth_replenish_buffer_pool(struct ibmveth_adapter *adapter, struc
 		dma_addr = dma_map_single(&adapter->vdev->dev, skb->data,
 				pool->buff_size, DMA_FROM_DEVICE);
 
+		if (dma_mapping_error((&adapter->vdev->dev, dma_addr))
+			goto failure;
+
 		pool->free_map[free_index] = IBM_VETH_INVALID_MAP;
 		pool->dma_addr[index] = dma_addr;
 		pool->skbuff[index] = skb;
@@ -267,20 +275,9 @@ static void ibmveth_replenish_buffer_pool(struct ibmveth_adapter *adapter, struc
 
 		lpar_rc = h_add_logical_lan_buffer(adapter->vdev->unit_address, desc.desc);
 
-		if(lpar_rc != H_SUCCESS) {
-			pool->free_map[free_index] = index;
-			pool->skbuff[index] = NULL;
-			if (pool->consumer_index == 0)
-				pool->consumer_index = pool->size - 1;
-			else
-				pool->consumer_index--;
-			dma_unmap_single(&adapter->vdev->dev,
-					pool->dma_addr[index], pool->buff_size,
-					DMA_FROM_DEVICE);
-			dev_kfree_skb_any(skb);
-			adapter->replenish_add_buff_failure++;
-			break;
-		} else {
+		if (lpar_rc != H_SUCCESS)
+			goto failure;
+		else {
 			buffers_added++;
 			adapter->replenish_add_buff_success++;
 		}
@@ -288,6 +285,24 @@ static void ibmveth_replenish_buffer_pool(struct ibmveth_adapter *adapter, struc
 
 	mb();
 	atomic_add(buffers_added, &(pool->available));
+	return;
+
+failure:
+	pool->free_map[free_index] = index;
+	pool->skbuff[index] = NULL;
+	if (pool->consumer_index == 0)
+		pool->consumer_index = pool->size - 1;
+	else
+		pool->consumer_index--;
+	if (!dma_mapping_error((&adapter->vdev->dev, dma_addr))
+		dma_unmap_single(&adapter->vdev->dev,
+		                 pool->dma_addr[index], pool->buff_size,
+		                 DMA_FROM_DEVICE);
+	dev_kfree_skb_any(skb);
+	adapter->replenish_add_buff_failure++;
+
+	mb();
+	atomic_add(buffers_added, &(pool->available));
 }
 
 /* replenish routine */
@@ -297,7 +312,7 @@ static void ibmveth_replenish_task(struct ibmveth_adapter *adapter)
 
 	adapter->replenish_task_cycles++;
 
-	for(i = 0; i < IbmVethNumBufferPools; i++)
+	for (i = (IbmVethNumBufferPools - 1); i >= 0; i--)
 		if(adapter->rx_buff_pool[i].active)
 			ibmveth_replenish_buffer_pool(adapter,
 						     &adapter->rx_buff_pool[i]);
@@ -433,11 +448,11 @@ static void ibmveth_rxq_harvest_buffer(struct ibmveth_adapter *adapter)
 static void ibmveth_cleanup(struct ibmveth_adapter *adapter)
 {
 	int i;
+	struct device *dev = &adapter->vdev->dev;
 
 	if(adapter->buffer_list_addr != NULL) {
-		if(!dma_mapping_error(adapter->buffer_list_dma)) {
-			dma_unmap_single(&adapter->vdev->dev,
-					adapter->buffer_list_dma, 4096,
+		if (!dma_mapping_error(dev, adapter->buffer_list_dma)) {
+			dma_unmap_single(dev, adapter->buffer_list_dma, 4096,
 					DMA_BIDIRECTIONAL);
 			adapter->buffer_list_dma = DMA_ERROR_CODE;
 		}
@@ -446,9 +461,8 @@ static void ibmveth_cleanup(struct ibmveth_adapter *adapter)
 	}
 
 	if(adapter->filter_list_addr != NULL) {
-		if(!dma_mapping_error(adapter->filter_list_dma)) {
-			dma_unmap_single(&adapter->vdev->dev,
-					adapter->filter_list_dma, 4096,
+		if (!dma_mapping_error(dev, adapter->filter_list_dma)) {
+			dma_unmap_single(dev, adapter->filter_list_dma, 4096,
 					DMA_BIDIRECTIONAL);
 			adapter->filter_list_dma = DMA_ERROR_CODE;
 		}
@@ -457,8 +471,8 @@ static void ibmveth_cleanup(struct ibmveth_adapter *adapter)
 	}
 
 	if(adapter->rx_queue.queue_addr != NULL) {
-		if(!dma_mapping_error(adapter->rx_queue.queue_dma)) {
-			dma_unmap_single(&adapter->vdev->dev,
+		if (!dma_mapping_error(dev, adapter->rx_queue.queue_dma)) {
+			dma_unmap_single(dev,
 					adapter->rx_queue.queue_dma,
 					adapter->rx_queue.queue_len,
 					DMA_BIDIRECTIONAL);
@@ -472,6 +486,18 @@ static void ibmveth_cleanup(struct ibmveth_adapter *adapter)
 		if (adapter->rx_buff_pool[i].active)
 			ibmveth_free_buffer_pool(adapter,
 						 &adapter->rx_buff_pool[i]);
+
+	if (adapter->bounce_buffer != NULL) {
+		if (!dma_mapping_error(adapter->bounce_buffer_dma)) {
+			dma_unmap_single(&adapter->vdev->dev,
+					adapter->bounce_buffer_dma,
+					adapter->netdev->mtu + IBMVETH_BUFF_OH,
+					DMA_BIDIRECTIONAL);
+			adapter->bounce_buffer_dma = DMA_ERROR_CODE;
+		}
+		kfree(adapter->bounce_buffer);
+		adapter->bounce_buffer = NULL;
+	}
 }
 
 static int ibmveth_register_logical_lan(struct ibmveth_adapter *adapter,
@@ -508,6 +534,7 @@ static int ibmveth_open(struct net_device *netdev)
 	int rc;
 	union ibmveth_buf_desc rxq_desc;
 	int i;
+	struct device *dev;
 
 	ibmveth_debug_printk("open starting\n");
 
@@ -536,17 +563,19 @@ static int ibmveth_open(struct net_device *netdev)
 		return -ENOMEM;
 	}
 
-	adapter->buffer_list_dma = dma_map_single(&adapter->vdev->dev,
+	dev = &adapter->vdev->dev;
+
+	adapter->buffer_list_dma = dma_map_single(dev,
 			adapter->buffer_list_addr, 4096, DMA_BIDIRECTIONAL);
-	adapter->filter_list_dma = dma_map_single(&adapter->vdev->dev,
+	adapter->filter_list_dma = dma_map_single(dev,
 			adapter->filter_list_addr, 4096, DMA_BIDIRECTIONAL);
-	adapter->rx_queue.queue_dma = dma_map_single(&adapter->vdev->dev,
+	adapter->rx_queue.queue_dma = dma_map_single(dev,
 			adapter->rx_queue.queue_addr,
 			adapter->rx_queue.queue_len, DMA_BIDIRECTIONAL);
 
-	if((dma_mapping_error(adapter->buffer_list_dma) ) ||
-	   (dma_mapping_error(adapter->filter_list_dma)) ||
-	   (dma_mapping_error(adapter->rx_queue.queue_dma))) {
+	if ((dma_mapping_error(dev, adapter->buffer_list_dma)) ||
+	    (dma_mapping_error(dev, adapter->filter_list_dma)) ||
+	    (dma_mapping_error(dev, adapter->rx_queue.queue_dma))) {
 		ibmveth_error_printk("unable to map filter or buffer list pages\n");
 		ibmveth_cleanup(adapter);
 		napi_disable(&adapter->napi);
@@ -607,6 +636,24 @@ static int ibmveth_open(struct net_device *netdev)
 		return rc;
 	}
 
+	adapter->bounce_buffer =
+	    kmalloc(netdev->mtu + IBMVETH_BUFF_OH, GFP_KERNEL);
+	if (!adapter->bounce_buffer) {
+		ibmveth_error_printk("unable to allocate bounce buffer\n");
+		ibmveth_cleanup(adapter);
+		napi_disable(&adapter->napi);
+		return -ENOMEM;
+	}
+	adapter->bounce_buffer_dma =
+	    dma_map_single(&adapter->vdev->dev, adapter->bounce_buffer,
+			   netdev->mtu + IBMVETH_BUFF_OH, DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, adapter->bounce_buffer_dma)) {
+		ibmveth_error_printk("unable to map bounce buffer\n");
+		ibmveth_cleanup(adapter);
+		napi_disable(&adapter->napi);
+		return -ENOMEM;
+	}
+
 	ibmveth_debug_printk("initial replenish cycle\n");
 	ibmveth_interrupt(netdev->irq, netdev);
 
@@ -853,10 +900,12 @@ static int ibmveth_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 	unsigned int tx_packets = 0;
 	unsigned int tx_send_failed = 0;
 	unsigned int tx_map_failed = 0;
+	int used_bounce = 0;
+	unsigned long data_dma_addr;
 
 	desc.fields.flags_len = IBMVETH_BUF_VALID | skb->len;
-	desc.fields.address = dma_map_single(&adapter->vdev->dev, skb->data,
-					     skb->len, DMA_TO_DEVICE);
+	data_dma_addr = dma_map_single(&adapter->vdev->dev, skb->data,
+				       skb->len, DMA_TO_DEVICE);
 
 	if (skb->ip_summed == CHECKSUM_PARTIAL &&
 	    ip_hdr(skb)->protocol != IPPROTO_TCP && skb_checksum_help(skb)) {
@@ -875,12 +924,16 @@ static int ibmveth_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 		buf[1] = 0;
 	}
 
-	if (dma_mapping_error(desc.fields.address)) {
-		ibmveth_error_printk("tx: unable to map xmit buffer\n");
+	if (dma_mapping_error((&adapter->vdev->dev, data_dma_addr)) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			ibmveth_error_printk("tx: unable to map xmit buffer\n");
+		skb_copy_from_linear_data(skb, adapter->bounce_buffer,
+					  skb->len);
+		desc.fields.address = adapter->bounce_buffer_dma;
 		tx_map_failed++;
-		tx_dropped++;
-		goto out;
-	}
+		used_bounce = 1;
+	} else
+		desc.fields.address = data_dma_addr;
 
 	/* send the frame. Arbitrarily set retrycount to 1024 */
 	correlator = 0;
@@ -904,8 +957,9 @@ static int ibmveth_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 		netdev->trans_start = jiffies;
 	}
 
-	dma_unmap_single(&adapter->vdev->dev, desc.fields.address,
-			 skb->len, DMA_TO_DEVICE);
+	if (!used_bounce)
+		dma_unmap_single(&adapter->vdev->dev, data_dma_addr,
+				 skb->len, DMA_TO_DEVICE);
 
 out:	spin_lock_irqsave(&adapter->stats_lock, flags);
 	netdev->stats.tx_dropped += tx_dropped;
@@ -1053,9 +1107,9 @@ static void ibmveth_set_multicast_list(struct net_device *netdev)
 static int ibmveth_change_mtu(struct net_device *dev, int new_mtu)
 {
 	struct ibmveth_adapter *adapter = dev->priv;
+	struct vio_dev *viodev = adapter->vdev;
 	int new_mtu_oh = new_mtu + IBMVETH_BUFF_OH;
-	int reinit = 0;
-	int i, rc;
+	int i;
 
 	if (new_mtu < IBMVETH_MAX_MTU)
 		return -EINVAL;
@@ -1067,23 +1121,34 @@ static int ibmveth_change_mtu(struct net_device *dev, int new_mtu)
 	if (i == IbmVethNumBufferPools)
 		return -EINVAL;
 
+	/* Deactivate all the buffer pools so that the next loop can activate
+	   only the buffer pools necessary to hold the new MTU */
+	for (i = 0; i < IbmVethNumBufferPools; i++)
+		if (adapter->rx_buff_pool[i].active) {
+			ibmveth_free_buffer_pool(adapter,
+						 &adapter->rx_buff_pool[i]);
+			adapter->rx_buff_pool[i].active = 0;
+		}
+
 	/* Look for an active buffer pool that can hold the new MTU */
 	for(i = 0; i<IbmVethNumBufferPools; i++) {
-		if (!adapter->rx_buff_pool[i].active) {
-			adapter->rx_buff_pool[i].active = 1;
-			reinit = 1;
-		}
+		adapter->rx_buff_pool[i].active = 1;
 
 		if (new_mtu_oh < adapter->rx_buff_pool[i].buff_size) {
-			if (reinit && netif_running(adapter->netdev)) {
+			if (netif_running(adapter->netdev)) {
 				adapter->pool_config = 1;
 				ibmveth_close(adapter->netdev);
 				adapter->pool_config = 0;
 				dev->mtu = new_mtu;
-				if ((rc = ibmveth_open(adapter->netdev)))
-					return rc;
-			} else
-				dev->mtu = new_mtu;
+				vio_cmo_set_dev_desired(viodev,
+						ibmveth_get_desired_dma
+						(viodev));
+				return ibmveth_open(adapter->netdev);
+			}
+			dev->mtu = new_mtu;
+			vio_cmo_set_dev_desired(viodev,
+						ibmveth_get_desired_dma
+						(viodev));
 			return 0;
 		}
 	}
@@ -1098,6 +1163,46 @@ static void ibmveth_poll_controller(struct net_device *dev)
 }
 #endif
 
+/**
+ * ibmveth_get_desired_dma - Calculate IO memory desired by the driver
+ *
+ * @vdev: struct vio_dev for the device whose desired IO mem is to be returned
+ *
+ * Return value:
+ *	Number of bytes of IO data the driver will need to perform well.
+ */
+static unsigned long ibmveth_get_desired_dma(struct vio_dev *vdev)
+{
+	struct net_device *netdev = dev_get_drvdata(&vdev->dev);
+	struct ibmveth_adapter *adapter;
+	unsigned long ret;
+	int i;
+	int rxqentries = 1;
+
+	/* netdev inits at probe time along with the structures we need below*/
+	if (netdev == NULL)
+		return IOMMU_PAGE_ALIGN(IBMVETH_IO_ENTITLEMENT_DEFAULT);
+
+	adapter = netdev_priv(netdev);
+
+	ret = IBMVETH_BUFF_LIST_SIZE + IBMVETH_FILT_LIST_SIZE;
+	ret += IOMMU_PAGE_ALIGN(netdev->mtu);
+
+	for (i = 0; i < IbmVethNumBufferPools; i++) {
+		/* add the size of the active receive buffers */
+		if (adapter->rx_buff_pool[i].active)
+			ret +=
+			    adapter->rx_buff_pool[i].size *
+			    IOMMU_PAGE_ALIGN(adapter->rx_buff_pool[i].
+			            buff_size);
+		rxqentries += adapter->rx_buff_pool[i].size;
+	}
+	/* add the size of the receive queue entries */
+	ret += IOMMU_PAGE_ALIGN(rxqentries * sizeof(struct ibmveth_rx_q_entry));
+
+	return ret;
+}
+
 static int __devinit ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 {
 	int rc, i;
@@ -1242,6 +1347,8 @@ static int __devexit ibmveth_remove(struct vio_dev *dev)
 	ibmveth_proc_unregister_adapter(adapter);
 
 	free_netdev(netdev);
+	dev_set_drvdata(&dev->dev, NULL);
+
 	return 0;
 }
 
@@ -1402,14 +1509,15 @@ const char * buf, size_t count)
 				return -EPERM;
 			}
 
-			pool->active = 0;
 			if (netif_running(netdev)) {
 				adapter->pool_config = 1;
 				ibmveth_close(netdev);
+				pool->active = 0;
 				adapter->pool_config = 0;
 				if ((rc = ibmveth_open(netdev)))
 					return rc;
 			}
+			pool->active = 0;
 		}
 	} else if (attr == &veth_num_attr) {
 		if (value <= 0 || value > IBMVETH_MAX_POOL_COUNT)
@@ -1485,6 +1593,7 @@ static struct vio_driver ibmveth_driver = {
 	.id_table	= ibmveth_device_table,
 	.probe		= ibmveth_probe,
 	.remove		= ibmveth_remove,
+	.get_desired_dma = ibmveth_get_desired_dma,
 	.driver		= {
 		.name	= ibmveth_driver_name,
 		.owner	= THIS_MODULE,
diff --git a/drivers/net/ibmveth.h b/drivers/net/ibmveth.h
index 41f61cd1885..d2818694875 100644
--- a/drivers/net/ibmveth.h
+++ b/drivers/net/ibmveth.h
@@ -93,9 +93,12 @@ static inline long h_illan_attributes(unsigned long unit_address,
   plpar_hcall_norets(H_CHANGE_LOGICAL_LAN_MAC, ua, mac)
 
 #define IbmVethNumBufferPools 5
+#define IBMVETH_IO_ENTITLEMENT_DEFAULT 4243456 /* MTU of 1500 needs 4.2Mb */
 #define IBMVETH_BUFF_OH 22 /* Overhead: 14 ethernet header + 8 opaque handle */
 #define IBMVETH_MAX_MTU 68
 #define IBMVETH_MAX_POOL_COUNT 4096
+#define IBMVETH_BUFF_LIST_SIZE 4096
+#define IBMVETH_FILT_LIST_SIZE 4096
 #define IBMVETH_MAX_BUF_SIZE (1024 * 128)
 
 static int pool_size[] = { 512, 1024 * 2, 1024 * 16, 1024 * 32, 1024 * 64 };
@@ -143,6 +146,8 @@ struct ibmveth_adapter {
     struct ibmveth_rx_q rx_queue;
     int pool_config;
     int rx_csum;
+    void *bounce_buffer;
+    dma_addr_t bounce_buffer_dma;
 
     /* adapter specific stats */
     u64 replenish_task_cycles;
diff --git a/drivers/net/iseries_veth.c b/drivers/net/iseries_veth.c
index b8d0639c1cd..c46864d626b 100644
--- a/drivers/net/iseries_veth.c
+++ b/drivers/net/iseries_veth.c
@@ -1128,7 +1128,7 @@ static int veth_transmit_to_one(struct sk_buff *skb, HvLpIndex rlp,
 	msg->data.addr[0] = dma_map_single(port->dev, skb->data,
 				skb->len, DMA_TO_DEVICE);
 
-	if (dma_mapping_error(msg->data.addr[0]))
+	if (dma_mapping_error(port->dev, msg->data.addr[0]))
 		goto recycle_and_drop;
 
 	msg->dev = port->dev;
@@ -1226,7 +1226,7 @@ static void veth_recycle_msg(struct veth_lpar_connection *cnx,
 		dma_address = msg->data.addr[0];
 		dma_length = msg->data.len[0];
 
-		if (!dma_mapping_error(dma_address))
+		if (!dma_mapping_error(msg->dev, dma_address))
 			dma_unmap_single(msg->dev, dma_address, dma_length,
 					DMA_TO_DEVICE);
 
diff --git a/drivers/net/mlx4/alloc.c b/drivers/net/mlx4/alloc.c
index f9d6b4dca18..096bca54bcf 100644
--- a/drivers/net/mlx4/alloc.c
+++ b/drivers/net/mlx4/alloc.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/mlx4/catas.c b/drivers/net/mlx4/catas.c
index aa952877904..f094ee00c41 100644
--- a/drivers/net/mlx4/catas.c
+++ b/drivers/net/mlx4/catas.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/mlx4/cmd.c b/drivers/net/mlx4/cmd.c
index 70dff94a8bc..2845a0560b8 100644
--- a/drivers/net/mlx4/cmd.c
+++ b/drivers/net/mlx4/cmd.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems, Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -67,6 +67,8 @@ enum {
 	CMD_STAT_BAD_INDEX	= 0x0a,
 	/* FW image corrupted: */
 	CMD_STAT_BAD_NVMEM	= 0x0b,
+	/* Error in ICM mapping (e.g. not enough auxiliary ICM pages to execute command): */
+	CMD_STAT_ICM_ERROR	= 0x0c,
 	/* Attempt to modify a QP/EE which is not in the presumed state: */
 	CMD_STAT_BAD_QP_STATE   = 0x10,
 	/* Bad segment parameters (Address/Size): */
@@ -119,6 +121,7 @@ static int mlx4_status_to_errno(u8 status)
 		[CMD_STAT_BAD_RES_STATE]  = -EBADF,
 		[CMD_STAT_BAD_INDEX]	  = -EBADF,
 		[CMD_STAT_BAD_NVMEM]	  = -EFAULT,
+		[CMD_STAT_ICM_ERROR]	  = -ENFILE,
 		[CMD_STAT_BAD_QP_STATE]   = -EINVAL,
 		[CMD_STAT_BAD_SEG_PARAM]  = -EFAULT,
 		[CMD_STAT_REG_BOUND]	  = -EBUSY,
diff --git a/drivers/net/mlx4/cq.c b/drivers/net/mlx4/cq.c
index 95e87a2f889..9bb50e3f897 100644
--- a/drivers/net/mlx4/cq.c
+++ b/drivers/net/mlx4/cq.c
@@ -2,7 +2,7 @@
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
  * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems, Inc. All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2004 Voltaire, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
diff --git a/drivers/net/mlx4/eq.c b/drivers/net/mlx4/eq.c
index e141a1513f0..8a8b56135a5 100644
--- a/drivers/net/mlx4/eq.c
+++ b/drivers/net/mlx4/eq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -33,6 +33,7 @@
 
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/mm.h>
 #include <linux/dma-mapping.h>
 
 #include <linux/mlx4/cmd.h>
@@ -525,7 +526,7 @@ int mlx4_map_eq_icm(struct mlx4_dev *dev, u64 icm_virt)
 		return -ENOMEM;
 	priv->eq_table.icm_dma  = pci_map_page(dev->pdev, priv->eq_table.icm_page, 0,
 					       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-	if (pci_dma_mapping_error(priv->eq_table.icm_dma)) {
+	if (pci_dma_mapping_error(dev->pdev, priv->eq_table.icm_dma)) {
 		__free_page(priv->eq_table.icm_page);
 		return -ENOMEM;
 	}
diff --git a/drivers/net/mlx4/fw.c b/drivers/net/mlx4/fw.c
index 2b5006b9be6..7e32955da98 100644
--- a/drivers/net/mlx4/fw.c
+++ b/drivers/net/mlx4/fw.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems, Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -46,6 +46,10 @@ enum {
 extern void __buggy_use_of_MLX4_GET(void);
 extern void __buggy_use_of_MLX4_PUT(void);
 
+static int enable_qos;
+module_param(enable_qos, bool, 0444);
+MODULE_PARM_DESC(enable_qos, "Enable Quality of Service support in the HCA (default: off)");
+
 #define MLX4_GET(dest, source, offset)				      \
 	do {							      \
 		void *__p = (char *) (source) + (offset);	      \
@@ -198,7 +202,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_C_MPT_ENTRY_SZ_OFFSET	0x8e
 #define QUERY_DEV_CAP_MTT_ENTRY_SZ_OFFSET	0x90
 #define QUERY_DEV_CAP_D_MPT_ENTRY_SZ_OFFSET	0x92
-#define QUERY_DEV_CAP_BMME_FLAGS_OFFSET		0x97
+#define QUERY_DEV_CAP_BMME_FLAGS_OFFSET		0x94
 #define QUERY_DEV_CAP_RSVD_LKEY_OFFSET		0x98
 #define QUERY_DEV_CAP_MAX_ICM_SZ_OFFSET		0xa0
 
@@ -373,12 +377,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		}
 	}
 
-	if (dev_cap->bmme_flags & 1)
-		mlx4_dbg(dev, "Base MM extensions: yes "
-			 "(flags %d, rsvd L_Key %08x)\n",
-			 dev_cap->bmme_flags, dev_cap->reserved_lkey);
-	else
-		mlx4_dbg(dev, "Base MM extensions: no\n");
+	mlx4_dbg(dev, "Base MM extensions: flags %08x, rsvd L_Key %08x\n",
+		 dev_cap->bmme_flags, dev_cap->reserved_lkey);
 
 	/*
 	 * Each UAR has 4 EQ doorbells; so if a UAR is reserved, then
@@ -737,6 +737,10 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_IPOIB_CSUM)
 		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 3);
 
+	/* Enable QoS support if module parameter set */
+	if (enable_qos)
+		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 2);
+
 	/* QPC/EEC/CQC/EQC/RDMARC attributes */
 
 	MLX4_PUT(inbox, param->qpc_base,      INIT_HCA_QPC_BASE_OFFSET);
diff --git a/drivers/net/mlx4/fw.h b/drivers/net/mlx4/fw.h
index a0e046c149b..decbb5c2ad4 100644
--- a/drivers/net/mlx4/fw.h
+++ b/drivers/net/mlx4/fw.h
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2006, 2007 Cisco Systems.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -98,7 +98,7 @@ struct mlx4_dev_cap {
 	int cmpt_entry_sz;
 	int mtt_entry_sz;
 	int resize_srq;
-	u8  bmme_flags;
+	u32 bmme_flags;
 	u32 reserved_lkey;
 	u64 max_icm_sz;
 	int max_gso_sz;
diff --git a/drivers/net/mlx4/icm.c b/drivers/net/mlx4/icm.c
index 2a5bef6388f..baf4bf66062 100644
--- a/drivers/net/mlx4/icm.c
+++ b/drivers/net/mlx4/icm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
diff --git a/drivers/net/mlx4/icm.h b/drivers/net/mlx4/icm.h
index 6c44edf3584..ab56a2f89b6 100644
--- a/drivers/net/mlx4/icm.h
+++ b/drivers/net/mlx4/icm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
diff --git a/drivers/net/mlx4/intf.c b/drivers/net/mlx4/intf.c
index 4a6c4d526f1..0e7eb1038f9 100644
--- a/drivers/net/mlx4/intf.c
+++ b/drivers/net/mlx4/intf.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/mlx4/main.c b/drivers/net/mlx4/main.c
index d3736013fe9..1252a919de2 100644
--- a/drivers/net/mlx4/main.c
+++ b/drivers/net/mlx4/main.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
  * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2006, 2007 Cisco Systems, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -158,6 +158,8 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.max_msg_sz         = dev_cap->max_msg_sz;
 	dev->caps.page_size_cap	     = ~(u32) (dev_cap->min_page_sz - 1);
 	dev->caps.flags		     = dev_cap->flags;
+	dev->caps.bmme_flags	     = dev_cap->bmme_flags;
+	dev->caps.reserved_lkey	     = dev_cap->reserved_lkey;
 	dev->caps.stat_rate_support  = dev_cap->stat_rate_support;
 	dev->caps.max_gso_sz	     = dev_cap->max_gso_sz;
 
diff --git a/drivers/net/mlx4/mcg.c b/drivers/net/mlx4/mcg.c
index b4b57870ddf..c83f88ce073 100644
--- a/drivers/net/mlx4/mcg.c
+++ b/drivers/net/mlx4/mcg.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/mlx4/mlx4.h b/drivers/net/mlx4/mlx4.h
index a4023c2dd05..5337e3ac3e7 100644
--- a/drivers/net/mlx4/mlx4.h
+++ b/drivers/net/mlx4/mlx4.h
@@ -2,7 +2,7 @@
  * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
  * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2004 Voltaire, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -118,6 +118,7 @@ struct mlx4_bitmap {
 
 struct mlx4_buddy {
 	unsigned long	      **bits;
+	unsigned int	       *num_free;
 	int			max_order;
 	spinlock_t		lock;
 };
diff --git a/drivers/net/mlx4/mr.c b/drivers/net/mlx4/mr.c
index 03a9abcce52..62071d9c4a5 100644
--- a/drivers/net/mlx4/mr.c
+++ b/drivers/net/mlx4/mr.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2004 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -47,7 +47,7 @@ struct mlx4_mpt_entry {
 	__be32 flags;
 	__be32 qpn;
 	__be32 key;
-	__be32 pd;
+	__be32 pd_flags;
 	__be64 start;
 	__be64 length;
 	__be32 lkey;
@@ -61,11 +61,15 @@ struct mlx4_mpt_entry {
 } __attribute__((packed));
 
 #define MLX4_MPT_FLAG_SW_OWNS	    (0xfUL << 28)
+#define MLX4_MPT_FLAG_FREE	    (0x3UL << 28)
 #define MLX4_MPT_FLAG_MIO	    (1 << 17)
 #define MLX4_MPT_FLAG_BIND_ENABLE   (1 << 15)
 #define MLX4_MPT_FLAG_PHYSICAL	    (1 <<  9)
 #define MLX4_MPT_FLAG_REGION	    (1 <<  8)
 
+#define MLX4_MPT_PD_FLAG_FAST_REG   (1 << 26)
+#define MLX4_MPT_PD_FLAG_EN_INV	    (3 << 24)
+
 #define MLX4_MTT_FLAG_PRESENT		1
 
 #define MLX4_MPT_STATUS_SW		0xF0
@@ -79,23 +83,26 @@ static u32 mlx4_buddy_alloc(struct mlx4_buddy *buddy, int order)
 
 	spin_lock(&buddy->lock);
 
-	for (o = order; o <= buddy->max_order; ++o) {
-		m = 1 << (buddy->max_order - o);
-		seg = find_first_bit(buddy->bits[o], m);
-		if (seg < m)
-			goto found;
-	}
+	for (o = order; o <= buddy->max_order; ++o)
+		if (buddy->num_free[o]) {
+			m = 1 << (buddy->max_order - o);
+			seg = find_first_bit(buddy->bits[o], m);
+			if (seg < m)
+				goto found;
+		}
 
 	spin_unlock(&buddy->lock);
 	return -1;
 
  found:
 	clear_bit(seg, buddy->bits[o]);
+	--buddy->num_free[o];
 
 	while (o > order) {
 		--o;
 		seg <<= 1;
 		set_bit(seg ^ 1, buddy->bits[o]);
+		++buddy->num_free[o];
 	}
 
 	spin_unlock(&buddy->lock);
@@ -113,11 +120,13 @@ static void mlx4_buddy_free(struct mlx4_buddy *buddy, u32 seg, int order)
 
 	while (test_bit(seg ^ 1, buddy->bits[order])) {
 		clear_bit(seg ^ 1, buddy->bits[order]);
+		--buddy->num_free[order];
 		seg >>= 1;
 		++order;
 	}
 
 	set_bit(seg, buddy->bits[order]);
+	++buddy->num_free[order];
 
 	spin_unlock(&buddy->lock);
 }
@@ -131,7 +140,9 @@ static int mlx4_buddy_init(struct mlx4_buddy *buddy, int max_order)
 
 	buddy->bits = kzalloc((buddy->max_order + 1) * sizeof (long *),
 			      GFP_KERNEL);
-	if (!buddy->bits)
+	buddy->num_free = kzalloc((buddy->max_order + 1) * sizeof (int *),
+				  GFP_KERNEL);
+	if (!buddy->bits || !buddy->num_free)
 		goto err_out;
 
 	for (i = 0; i <= buddy->max_order; ++i) {
@@ -143,6 +154,7 @@ static int mlx4_buddy_init(struct mlx4_buddy *buddy, int max_order)
 	}
 
 	set_bit(0, buddy->bits[buddy->max_order]);
+	buddy->num_free[buddy->max_order] = 1;
 
 	return 0;
 
@@ -150,9 +162,10 @@ err_out_free:
 	for (i = 0; i <= buddy->max_order; ++i)
 		kfree(buddy->bits[i]);
 
+err_out:
 	kfree(buddy->bits);
+	kfree(buddy->num_free);
 
-err_out:
 	return -ENOMEM;
 }
 
@@ -164,6 +177,7 @@ static void mlx4_buddy_cleanup(struct mlx4_buddy *buddy)
 		kfree(buddy->bits[i]);
 
 	kfree(buddy->bits);
+	kfree(buddy->num_free);
 }
 
 static u32 mlx4_alloc_mtt_range(struct mlx4_dev *dev, int order)
@@ -314,21 +328,30 @@ int mlx4_mr_enable(struct mlx4_dev *dev, struct mlx4_mr *mr)
 
 	memset(mpt_entry, 0, sizeof *mpt_entry);
 
-	mpt_entry->flags = cpu_to_be32(MLX4_MPT_FLAG_SW_OWNS	 |
-				       MLX4_MPT_FLAG_MIO	 |
+	mpt_entry->flags = cpu_to_be32(MLX4_MPT_FLAG_MIO	 |
 				       MLX4_MPT_FLAG_REGION	 |
 				       mr->access);
 
 	mpt_entry->key	       = cpu_to_be32(key_to_hw_index(mr->key));
-	mpt_entry->pd	       = cpu_to_be32(mr->pd);
+	mpt_entry->pd_flags    = cpu_to_be32(mr->pd | MLX4_MPT_PD_FLAG_EN_INV);
 	mpt_entry->start       = cpu_to_be64(mr->iova);
 	mpt_entry->length      = cpu_to_be64(mr->size);
 	mpt_entry->entity_size = cpu_to_be32(mr->mtt.page_shift);
+
 	if (mr->mtt.order < 0) {
 		mpt_entry->flags |= cpu_to_be32(MLX4_MPT_FLAG_PHYSICAL);
 		mpt_entry->mtt_seg = 0;
-	} else
+	} else {
 		mpt_entry->mtt_seg = cpu_to_be64(mlx4_mtt_addr(dev, &mr->mtt));
+	}
+
+	if (mr->mtt.order >= 0 && mr->mtt.page_shift == 0) {
+		/* fast register MR in free state */
+		mpt_entry->flags    |= cpu_to_be32(MLX4_MPT_FLAG_FREE);
+		mpt_entry->pd_flags |= cpu_to_be32(MLX4_MPT_PD_FLAG_FAST_REG);
+	} else {
+		mpt_entry->flags    |= cpu_to_be32(MLX4_MPT_FLAG_SW_OWNS);
+	}
 
 	err = mlx4_SW2HW_MPT(dev, mailbox,
 			     key_to_hw_index(mr->key) & (dev->caps.num_mpts - 1));
diff --git a/drivers/net/mlx4/pd.c b/drivers/net/mlx4/pd.c
index 3a93c5f0f7a..aa616892d09 100644
--- a/drivers/net/mlx4/pd.c
+++ b/drivers/net/mlx4/pd.c
@@ -91,6 +91,13 @@ EXPORT_SYMBOL_GPL(mlx4_uar_free);
 
 int mlx4_init_uar_table(struct mlx4_dev *dev)
 {
+	if (dev->caps.num_uars <= 128) {
+		mlx4_err(dev, "Only %d UAR pages (need more than 128)\n",
+			 dev->caps.num_uars);
+		mlx4_err(dev, "Increase firmware log2_uar_bar_megabytes?\n");
+		return -ENODEV;
+	}
+
 	return mlx4_bitmap_init(&mlx4_priv(dev)->uar_table.bitmap,
 				dev->caps.num_uars, dev->caps.num_uars - 1,
 				max(128, dev->caps.reserved_uars));
diff --git a/drivers/net/mlx4/qp.c b/drivers/net/mlx4/qp.c
index ee5484c44a1..c49a86044bf 100644
--- a/drivers/net/mlx4/qp.c
+++ b/drivers/net/mlx4/qp.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2004 Topspin Communications.  All rights reserved.
  * Copyright (c) 2005, 2006, 2007 Cisco Systems, Inc. All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies. All rights reserved.
  * Copyright (c) 2004 Voltaire, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
diff --git a/drivers/net/mlx4/reset.c b/drivers/net/mlx4/reset.c
index e199715fabd..3951b884c0f 100644
--- a/drivers/net/mlx4/reset.c
+++ b/drivers/net/mlx4/reset.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc.  All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/mlx4/srq.c b/drivers/net/mlx4/srq.c
index d23f46d692e..533eb6db24b 100644
--- a/drivers/net/mlx4/srq.c
+++ b/drivers/net/mlx4/srq.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2006, 2007 Cisco Systems, Inc. All rights reserved.
+ * Copyright (c) 2007, 2008 Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/pasemi_mac.c b/drivers/net/pasemi_mac.c
index 993d87c9296..edc0fd58898 100644
--- a/drivers/net/pasemi_mac.c
+++ b/drivers/net/pasemi_mac.c
@@ -650,7 +650,7 @@ static void pasemi_mac_replenish_rx_ring(const struct net_device *dev,
 				     mac->bufsz - LOCAL_SKB_ALIGN,
 				     PCI_DMA_FROMDEVICE);
 
-		if (unlikely(dma_mapping_error(dma))) {
+		if (unlikely(pci_dma_mapping_error(mac->dma_pdev, dma))) {
 			dev_kfree_skb_irq(info->skb);
 			break;
 		}
@@ -1519,7 +1519,7 @@ static int pasemi_mac_start_tx(struct sk_buff *skb, struct net_device *dev)
 	map[0] = pci_map_single(mac->dma_pdev, skb->data, skb_headlen(skb),
 				PCI_DMA_TODEVICE);
 	map_size[0] = skb_headlen(skb);
-	if (dma_mapping_error(map[0]))
+	if (pci_dma_mapping_error(mac->dma_pdev, map[0]))
 		goto out_err_nolock;
 
 	for (i = 0; i < nfrags; i++) {
@@ -1529,7 +1529,7 @@ static int pasemi_mac_start_tx(struct sk_buff *skb, struct net_device *dev)
 					frag->page_offset, frag->size,
 					PCI_DMA_TODEVICE);
 		map_size[i+1] = frag->size;
-		if (dma_mapping_error(map[i+1])) {
+		if (pci_dma_mapping_error(mac->dma_pdev, map[i+1])) {
 			nfrags = i;
 			goto out_err_nolock;
 		}
diff --git a/drivers/net/ppp_generic.c b/drivers/net/ppp_generic.c
index 739b3ab7bcc..ddccc074a76 100644
--- a/drivers/net/ppp_generic.c
+++ b/drivers/net/ppp_generic.c
@@ -581,12 +581,12 @@ static long ppp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			if (file == ppp->owner)
 				ppp_shutdown_interface(ppp);
 		}
-		if (atomic_read(&file->f_count) <= 2) {
+		if (atomic_long_read(&file->f_count) <= 2) {
 			ppp_release(NULL, file);
 			err = 0;
 		} else
-			printk(KERN_DEBUG "PPPIOCDETACH file->f_count=%d\n",
-			       atomic_read(&file->f_count));
+			printk(KERN_DEBUG "PPPIOCDETACH file->f_count=%ld\n",
+			       atomic_long_read(&file->f_count));
 		unlock_kernel();
 		return err;
 	}
diff --git a/drivers/net/qla3xxx.c b/drivers/net/qla3xxx.c
index e7d48a352be..e82b37bbd6c 100644
--- a/drivers/net/qla3xxx.c
+++ b/drivers/net/qla3xxx.c
@@ -328,7 +328,7 @@ static void ql_release_to_lrg_buf_free_list(struct ql3_adapter *qdev,
 					     qdev->lrg_buffer_len -
 					     QL_HEADER_SPACE,
 					     PCI_DMA_FROMDEVICE);
-			err = pci_dma_mapping_error(map);
+			err = pci_dma_mapping_error(qdev->pdev, map);
 			if(err) {
 				printk(KERN_ERR "%s: PCI mapping failed with error: %d\n",
 				       qdev->ndev->name, err);
@@ -1919,7 +1919,7 @@ static int ql_populate_free_queue(struct ql3_adapter *qdev)
 						     QL_HEADER_SPACE,
 						     PCI_DMA_FROMDEVICE);
 
-				err = pci_dma_mapping_error(map);
+				err = pci_dma_mapping_error(qdev->pdev, map);
 				if(err) {
 					printk(KERN_ERR "%s: PCI mapping failed with error: %d\n",
 					       qdev->ndev->name, err);
@@ -2454,7 +2454,7 @@ static int ql_send_map(struct ql3_adapter *qdev,
 	 */
 	map = pci_map_single(qdev->pdev, skb->data, len, PCI_DMA_TODEVICE);
 
-	err = pci_dma_mapping_error(map);
+	err = pci_dma_mapping_error(qdev->pdev, map);
 	if(err) {
 		printk(KERN_ERR "%s: PCI mapping failed with error: %d\n",
 		       qdev->ndev->name, err);
@@ -2487,7 +2487,7 @@ static int ql_send_map(struct ql3_adapter *qdev,
 						     sizeof(struct oal),
 						     PCI_DMA_TODEVICE);
 
-				err = pci_dma_mapping_error(map);
+				err = pci_dma_mapping_error(qdev->pdev, map);
 				if(err) {
 
 					printk(KERN_ERR "%s: PCI mapping outbound address list with error: %d\n",
@@ -2514,7 +2514,7 @@ static int ql_send_map(struct ql3_adapter *qdev,
 					 frag->page_offset, frag->size,
 					 PCI_DMA_TODEVICE);
 
-			err = pci_dma_mapping_error(map);
+			err = pci_dma_mapping_error(qdev->pdev, map);
 			if(err) {
 				printk(KERN_ERR "%s: PCI mapping frags failed with error: %d\n",
 				       qdev->ndev->name, err);
@@ -2916,7 +2916,7 @@ static int ql_alloc_large_buffers(struct ql3_adapter *qdev)
 					     QL_HEADER_SPACE,
 					     PCI_DMA_FROMDEVICE);
 
-			err = pci_dma_mapping_error(map);
+			err = pci_dma_mapping_error(qdev->pdev, map);
 			if(err) {
 				printk(KERN_ERR "%s: PCI mapping failed with error: %d\n",
 				       qdev->ndev->name, err);
diff --git a/drivers/net/s2io.c b/drivers/net/s2io.c
index 9dae40ccf04..86d77d05190 100644
--- a/drivers/net/s2io.c
+++ b/drivers/net/s2io.c
@@ -2512,8 +2512,8 @@ static void stop_nic(struct s2io_nic *nic)
  *   Return Value:
  *  SUCCESS on success or an appropriate -ve value on failure.
  */
-
-static int fill_rx_buffers(struct ring_info *ring, int from_card_up)
+static int fill_rx_buffers(struct s2io_nic *nic, struct ring_info *ring,
+				int from_card_up)
 {
 	struct sk_buff *skb;
 	struct RxD_t *rxdp;
@@ -2602,7 +2602,8 @@ static int fill_rx_buffers(struct ring_info *ring, int from_card_up)
 			rxdp1->Buffer0_ptr = pci_map_single
 			    (ring->pdev, skb->data, size - NET_IP_ALIGN,
 				PCI_DMA_FROMDEVICE);
-			if(pci_dma_mapping_error(rxdp1->Buffer0_ptr))
+			if (pci_dma_mapping_error(nic->pdev,
+						rxdp1->Buffer0_ptr))
 				goto pci_map_failed;
 
 			rxdp->Control_2 =
@@ -2636,7 +2637,8 @@ static int fill_rx_buffers(struct ring_info *ring, int from_card_up)
 				rxdp3->Buffer0_ptr =
 				   pci_map_single(ring->pdev, ba->ba_0,
 					BUF0_LEN, PCI_DMA_FROMDEVICE);
-				if (pci_dma_mapping_error(rxdp3->Buffer0_ptr))
+			if (pci_dma_mapping_error(nic->pdev,
+						rxdp3->Buffer0_ptr))
 					goto pci_map_failed;
 			} else
 				pci_dma_sync_single_for_device(ring->pdev,
@@ -2655,7 +2657,8 @@ static int fill_rx_buffers(struct ring_info *ring, int from_card_up)
 				(ring->pdev, skb->data, ring->mtu + 4,
 						PCI_DMA_FROMDEVICE);
 
-				if (pci_dma_mapping_error(rxdp3->Buffer2_ptr))
+				if (pci_dma_mapping_error(nic->pdev,
+							rxdp3->Buffer2_ptr))
 					goto pci_map_failed;
 
 				if (from_card_up) {
@@ -2664,8 +2667,8 @@ static int fill_rx_buffers(struct ring_info *ring, int from_card_up)
 						ba->ba_1, BUF1_LEN,
 						PCI_DMA_FROMDEVICE);
 
-					if (pci_dma_mapping_error
-						(rxdp3->Buffer1_ptr)) {
+					if (pci_dma_mapping_error(nic->pdev,
+						rxdp3->Buffer1_ptr)) {
 						pci_unmap_single
 							(ring->pdev,
 						    (dma_addr_t)(unsigned long)
@@ -2806,9 +2809,9 @@ static void free_rx_buffers(struct s2io_nic *sp)
 	}
 }
 
-static int s2io_chk_rx_buffers(struct ring_info *ring)
+static int s2io_chk_rx_buffers(struct s2io_nic *nic, struct ring_info *ring)
 {
-	if (fill_rx_buffers(ring, 0) == -ENOMEM) {
+	if (fill_rx_buffers(nic, ring, 0) == -ENOMEM) {
 		DBG_PRINT(INFO_DBG, "%s:Out of memory", ring->dev->name);
 		DBG_PRINT(INFO_DBG, " in Rx Intr!!\n");
 	}
@@ -2848,7 +2851,7 @@ static int s2io_poll_msix(struct napi_struct *napi, int budget)
 		return 0;
 
 	pkts_processed = rx_intr_handler(ring, budget);
-	s2io_chk_rx_buffers(ring);
+	s2io_chk_rx_buffers(nic, ring);
 
 	if (pkts_processed < budget_org) {
 		netif_rx_complete(dev, napi);
@@ -2882,7 +2885,7 @@ static int s2io_poll_inta(struct napi_struct *napi, int budget)
 	for (i = 0; i < config->rx_ring_num; i++) {
 		ring = &mac_control->rings[i];
 		ring_pkts_processed = rx_intr_handler(ring, budget);
-		s2io_chk_rx_buffers(ring);
+		s2io_chk_rx_buffers(nic, ring);
 		pkts_processed += ring_pkts_processed;
 		budget -= ring_pkts_processed;
 		if (budget <= 0)
@@ -2939,7 +2942,8 @@ static void s2io_netpoll(struct net_device *dev)
 		rx_intr_handler(&mac_control->rings[i], 0);
 
 	for (i = 0; i < config->rx_ring_num; i++) {
-		if (fill_rx_buffers(&mac_control->rings[i], 0) == -ENOMEM) {
+		if (fill_rx_buffers(nic, &mac_control->rings[i], 0) ==
+				-ENOMEM) {
 			DBG_PRINT(INFO_DBG, "%s:Out of memory", dev->name);
 			DBG_PRINT(INFO_DBG, " in Rx Netpoll!!\n");
 			break;
@@ -4235,14 +4239,14 @@ static int s2io_xmit(struct sk_buff *skb, struct net_device *dev)
 		txdp->Buffer_Pointer = pci_map_single(sp->pdev,
 					fifo->ufo_in_band_v,
 					sizeof(u64), PCI_DMA_TODEVICE);
-		if (pci_dma_mapping_error(txdp->Buffer_Pointer))
+		if (pci_dma_mapping_error(sp->pdev, txdp->Buffer_Pointer))
 			goto pci_map_failed;
 		txdp++;
 	}
 
 	txdp->Buffer_Pointer = pci_map_single
 	    (sp->pdev, skb->data, frg_len, PCI_DMA_TODEVICE);
-	if (pci_dma_mapping_error(txdp->Buffer_Pointer))
+	if (pci_dma_mapping_error(sp->pdev, txdp->Buffer_Pointer))
 		goto pci_map_failed;
 
 	txdp->Host_Control = (unsigned long) skb;
@@ -4345,7 +4349,7 @@ static irqreturn_t s2io_msix_ring_handle(int irq, void *dev_id)
 		netif_rx_schedule(dev, &ring->napi);
 	} else {
 		rx_intr_handler(ring, 0);
-		s2io_chk_rx_buffers(ring);
+		s2io_chk_rx_buffers(sp, ring);
 	}
 
 	return IRQ_HANDLED;
@@ -4826,7 +4830,7 @@ static irqreturn_t s2io_isr(int irq, void *dev_id)
 		 */
 		if (!config->napi) {
 			for (i = 0; i < config->rx_ring_num; i++)
-				s2io_chk_rx_buffers(&mac_control->rings[i]);
+				s2io_chk_rx_buffers(sp, &mac_control->rings[i]);
 		}
 		writeq(sp->general_int_mask, &bar0->general_int_mask);
 		readl(&bar0->general_int_status);
@@ -6859,7 +6863,7 @@ static int set_rxd_buffer_pointer(struct s2io_nic *sp, struct RxD_t *rxdp,
 				pci_map_single( sp->pdev, (*skb)->data,
 					size - NET_IP_ALIGN,
 					PCI_DMA_FROMDEVICE);
-			if (pci_dma_mapping_error(rxdp1->Buffer0_ptr))
+			if (pci_dma_mapping_error(sp->pdev, rxdp1->Buffer0_ptr))
 				goto memalloc_failed;
 			rxdp->Host_Control = (unsigned long) (*skb);
 		}
@@ -6886,12 +6890,13 @@ static int set_rxd_buffer_pointer(struct s2io_nic *sp, struct RxD_t *rxdp,
 				pci_map_single(sp->pdev, (*skb)->data,
 					       dev->mtu + 4,
 					       PCI_DMA_FROMDEVICE);
-			if (pci_dma_mapping_error(rxdp3->Buffer2_ptr))
+			if (pci_dma_mapping_error(sp->pdev, rxdp3->Buffer2_ptr))
 				goto memalloc_failed;
 			rxdp3->Buffer0_ptr = *temp0 =
 				pci_map_single( sp->pdev, ba->ba_0, BUF0_LEN,
 						PCI_DMA_FROMDEVICE);
-			if (pci_dma_mapping_error(rxdp3->Buffer0_ptr)) {
+			if (pci_dma_mapping_error(sp->pdev,
+						rxdp3->Buffer0_ptr)) {
 				pci_unmap_single (sp->pdev,
 					(dma_addr_t)rxdp3->Buffer2_ptr,
 					dev->mtu + 4, PCI_DMA_FROMDEVICE);
@@ -6903,7 +6908,8 @@ static int set_rxd_buffer_pointer(struct s2io_nic *sp, struct RxD_t *rxdp,
 			rxdp3->Buffer1_ptr = *temp1 =
 				pci_map_single(sp->pdev, ba->ba_1, BUF1_LEN,
 						PCI_DMA_FROMDEVICE);
-			if (pci_dma_mapping_error(rxdp3->Buffer1_ptr)) {
+			if (pci_dma_mapping_error(sp->pdev,
+						rxdp3->Buffer1_ptr)) {
 				pci_unmap_single (sp->pdev,
 					(dma_addr_t)rxdp3->Buffer0_ptr,
 					BUF0_LEN, PCI_DMA_FROMDEVICE);
@@ -7187,7 +7193,7 @@ static int s2io_card_up(struct s2io_nic * sp)
 
 	for (i = 0; i < config->rx_ring_num; i++) {
 		mac_control->rings[i].mtu = dev->mtu;
-		ret = fill_rx_buffers(&mac_control->rings[i], 1);
+		ret = fill_rx_buffers(sp, &mac_control->rings[i], 1);
 		if (ret) {
 			DBG_PRINT(ERR_DBG, "%s: Out of memory in Open\n",
 				  dev->name);
diff --git a/drivers/net/sfc/rx.c b/drivers/net/sfc/rx.c
index 601b001437c..0d27dd39bc0 100644
--- a/drivers/net/sfc/rx.c
+++ b/drivers/net/sfc/rx.c
@@ -233,7 +233,7 @@ static inline int efx_init_rx_buffer_skb(struct efx_rx_queue *rx_queue,
 					  rx_buf->data, rx_buf->len,
 					  PCI_DMA_FROMDEVICE);
 
-	if (unlikely(pci_dma_mapping_error(rx_buf->dma_addr))) {
+	if (unlikely(pci_dma_mapping_error(efx->pci_dev, rx_buf->dma_addr))) {
 		dev_kfree_skb_any(rx_buf->skb);
 		rx_buf->skb = NULL;
 		return -EIO;
@@ -275,7 +275,7 @@ static inline int efx_init_rx_buffer_page(struct efx_rx_queue *rx_queue,
 					0, efx_rx_buf_size(efx),
 					PCI_DMA_FROMDEVICE);
 
-		if (unlikely(pci_dma_mapping_error(dma_addr))) {
+		if (unlikely(pci_dma_mapping_error(efx->pci_dev, dma_addr))) {
 			__free_pages(rx_buf->page, efx->rx_buffer_order);
 			rx_buf->page = NULL;
 			return -EIO;
diff --git a/drivers/net/sfc/tx.c b/drivers/net/sfc/tx.c
index 5cdd082ab8f..5e8374ab28e 100644
--- a/drivers/net/sfc/tx.c
+++ b/drivers/net/sfc/tx.c
@@ -172,7 +172,7 @@ static inline int efx_enqueue_skb(struct efx_tx_queue *tx_queue,
 
 	/* Process all fragments */
 	while (1) {
-		if (unlikely(pci_dma_mapping_error(dma_addr)))
+		if (unlikely(pci_dma_mapping_error(pci_dev, dma_addr)))
 			goto pci_err;
 
 		/* Store fields for marking in the per-fragment final
@@ -661,7 +661,8 @@ efx_tsoh_heap_alloc(struct efx_tx_queue *tx_queue, size_t header_len)
 	tsoh->dma_addr = pci_map_single(tx_queue->efx->pci_dev,
 					TSOH_BUFFER(tsoh), header_len,
 					PCI_DMA_TODEVICE);
-	if (unlikely(pci_dma_mapping_error(tsoh->dma_addr))) {
+	if (unlikely(pci_dma_mapping_error(tx_queue->efx->pci_dev,
+					   tsoh->dma_addr))) {
 		kfree(tsoh);
 		return NULL;
 	}
@@ -863,7 +864,7 @@ static inline int tso_get_fragment(struct tso_state *st, struct efx_nic *efx,
 
 	st->ifc.unmap_addr = pci_map_page(efx->pci_dev, page, page_off,
 					  len, PCI_DMA_TODEVICE);
-	if (likely(!pci_dma_mapping_error(st->ifc.unmap_addr))) {
+	if (likely(!pci_dma_mapping_error(efx->pci_dev, st->ifc.unmap_addr))) {
 		st->ifc.unmap_len = len;
 		st->ifc.len = len;
 		st->ifc.dma_addr = st->ifc.unmap_addr;
diff --git a/drivers/net/sky2.c b/drivers/net/sky2.c
index 711e4a8948e..5257cf464f1 100644
--- a/drivers/net/sky2.c
+++ b/drivers/net/sky2.c
@@ -1829,9 +1829,6 @@ static int sky2_down(struct net_device *dev)
 	if (netif_msg_ifdown(sky2))
 		printk(KERN_INFO PFX "%s: disabling interface\n", dev->name);
 
-	/* Stop more packets from being queued */
-	netif_stop_queue(dev);
-
 	/* Disable port IRQ */
 	imask = sky2_read32(hw, B0_IMSK);
 	imask &= ~portirq_msk[port];
@@ -1887,8 +1884,6 @@ static int sky2_down(struct net_device *dev)
 
 	sky2_phy_power_down(hw, port);
 
-	netif_carrier_off(dev);
-
 	/* turn off LED's */
 	sky2_write16(hw, B0_Y2LED, LED_STAT_OFF);
 
diff --git a/drivers/net/spider_net.c b/drivers/net/spider_net.c
index 00aa0b108cb..b6435d0d71f 100644
--- a/drivers/net/spider_net.c
+++ b/drivers/net/spider_net.c
@@ -452,7 +452,7 @@ spider_net_prepare_rx_descr(struct spider_net_card *card,
 	/* iommu-map the skb */
 	buf = pci_map_single(card->pdev, descr->skb->data,
 			SPIDER_NET_MAX_FRAME, PCI_DMA_FROMDEVICE);
-	if (pci_dma_mapping_error(buf)) {
+	if (pci_dma_mapping_error(card->pdev, buf)) {
 		dev_kfree_skb_any(descr->skb);
 		descr->skb = NULL;
 		if (netif_msg_rx_err(card) && net_ratelimit())
@@ -691,7 +691,7 @@ spider_net_prepare_tx_descr(struct spider_net_card *card,
 	unsigned long flags;
 
 	buf = pci_map_single(card->pdev, skb->data, skb->len, PCI_DMA_TODEVICE);
-	if (pci_dma_mapping_error(buf)) {
+	if (pci_dma_mapping_error(card->pdev, buf)) {
 		if (netif_msg_tx_err(card) && net_ratelimit())
 			dev_err(&card->netdev->dev, "could not iommu-map packet (%p, %i). "
 				  "Dropping packet\n", skb->data, skb->len);
diff --git a/drivers/net/tc35815.c b/drivers/net/tc35815.c
index a645e5028c1..8487ace9d2e 100644
--- a/drivers/net/tc35815.c
+++ b/drivers/net/tc35815.c
@@ -506,7 +506,7 @@ static void *alloc_rxbuf_page(struct pci_dev *hwdev, dma_addr_t *dma_handle)
 		return NULL;
 	*dma_handle = pci_map_single(hwdev, buf, PAGE_SIZE,
 				     PCI_DMA_FROMDEVICE);
-	if (pci_dma_mapping_error(*dma_handle)) {
+	if (pci_dma_mapping_error(hwdev, *dma_handle)) {
 		free_page((unsigned long)buf);
 		return NULL;
 	}
@@ -536,7 +536,7 @@ static struct sk_buff *alloc_rxbuf_skb(struct net_device *dev,
 		return NULL;
 	*dma_handle = pci_map_single(hwdev, skb->data, RX_BUF_SIZE,
 				     PCI_DMA_FROMDEVICE);
-	if (pci_dma_mapping_error(*dma_handle)) {
+	if (pci_dma_mapping_error(hwdev, *dma_handle)) {
 		dev_kfree_skb_any(skb);
 		return NULL;
 	}
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index c28d7cb2035..0196a0df902 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -19,6 +19,7 @@
 //#define DEBUG
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/ethtool.h>
 #include <linux/module.h>
 #include <linux/virtio.h>
 #include <linux/virtio_net.h>
@@ -54,9 +55,15 @@ struct virtnet_info
 	struct tasklet_struct tasklet;
 	bool free_in_tasklet;
 
+	/* I like... big packets and I cannot lie! */
+	bool big_packets;
+
 	/* Receive & send queues. */
 	struct sk_buff_head recv;
 	struct sk_buff_head send;
+
+	/* Chain pages by the private ptr. */
+	struct page *pages;
 };
 
 static inline struct virtio_net_hdr *skb_vnet_hdr(struct sk_buff *skb)
@@ -69,6 +76,23 @@ static inline void vnet_hdr_to_sg(struct scatterlist *sg, struct sk_buff *skb)
 	sg_init_one(sg, skb_vnet_hdr(skb), sizeof(struct virtio_net_hdr));
 }
 
+static void give_a_page(struct virtnet_info *vi, struct page *page)
+{
+	page->private = (unsigned long)vi->pages;
+	vi->pages = page;
+}
+
+static struct page *get_a_page(struct virtnet_info *vi, gfp_t gfp_mask)
+{
+	struct page *p = vi->pages;
+
+	if (p)
+		vi->pages = (struct page *)p->private;
+	else
+		p = alloc_page(gfp_mask);
+	return p;
+}
+
 static void skb_xmit_done(struct virtqueue *svq)
 {
 	struct virtnet_info *vi = svq->vdev->priv;
@@ -88,6 +112,7 @@ static void receive_skb(struct net_device *dev, struct sk_buff *skb,
 			unsigned len)
 {
 	struct virtio_net_hdr *hdr = skb_vnet_hdr(skb);
+	int err;
 
 	if (unlikely(len < sizeof(struct virtio_net_hdr) + ETH_HLEN)) {
 		pr_debug("%s: short packet %i\n", dev->name, len);
@@ -95,10 +120,23 @@ static void receive_skb(struct net_device *dev, struct sk_buff *skb,
 		goto drop;
 	}
 	len -= sizeof(struct virtio_net_hdr);
-	BUG_ON(len > MAX_PACKET_LEN);
 
-	skb_trim(skb, len);
+	if (len <= MAX_PACKET_LEN) {
+		unsigned int i;
 
+		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++)
+			give_a_page(dev->priv, skb_shinfo(skb)->frags[i].page);
+		skb->data_len = 0;
+		skb_shinfo(skb)->nr_frags = 0;
+	}
+
+	err = pskb_trim(skb, len);
+	if (err) {
+		pr_debug("%s: pskb_trim failed %i %d\n", dev->name, len, err);
+		dev->stats.rx_dropped++;
+		goto drop;
+	}
+	skb->truesize += skb->data_len;
 	dev->stats.rx_bytes += skb->len;
 	dev->stats.rx_packets++;
 
@@ -160,7 +198,7 @@ static void try_fill_recv(struct virtnet_info *vi)
 {
 	struct sk_buff *skb;
 	struct scatterlist sg[2+MAX_SKB_FRAGS];
-	int num, err;
+	int num, err, i;
 
 	sg_init_table(sg, 2+MAX_SKB_FRAGS);
 	for (;;) {
@@ -170,6 +208,24 @@ static void try_fill_recv(struct virtnet_info *vi)
 
 		skb_put(skb, MAX_PACKET_LEN);
 		vnet_hdr_to_sg(sg, skb);
+
+		if (vi->big_packets) {
+			for (i = 0; i < MAX_SKB_FRAGS; i++) {
+				skb_frag_t *f = &skb_shinfo(skb)->frags[i];
+				f->page = get_a_page(vi, GFP_ATOMIC);
+				if (!f->page)
+					break;
+
+				f->page_offset = 0;
+				f->size = PAGE_SIZE;
+
+				skb->data_len += PAGE_SIZE;
+				skb->len += PAGE_SIZE;
+
+				skb_shinfo(skb)->nr_frags++;
+			}
+		}
+
 		num = skb_to_sgvec(skb, sg+1, 0, skb->len) + 1;
 		skb_queue_head(&vi->recv, skb);
 
@@ -335,16 +391,11 @@ again:
 	free_old_xmit_skbs(vi);
 
 	/* If we has a buffer left over from last time, send it now. */
-	if (unlikely(vi->last_xmit_skb)) {
-		if (xmit_skb(vi, vi->last_xmit_skb) != 0) {
-			/* Drop this skb: we only queue one. */
-			vi->dev->stats.tx_dropped++;
-			kfree_skb(skb);
-			skb = NULL;
-			goto stop_queue;
-		}
-		vi->last_xmit_skb = NULL;
-	}
+	if (unlikely(vi->last_xmit_skb) &&
+	    xmit_skb(vi, vi->last_xmit_skb) != 0)
+		goto stop_queue;
+
+	vi->last_xmit_skb = NULL;
 
 	/* Put new one in send queue and do transmit */
 	if (likely(skb)) {
@@ -370,6 +421,11 @@ stop_queue:
 		netif_start_queue(dev);
 		goto again;
 	}
+	if (skb) {
+		/* Drop this skb: we only queue one. */
+		vi->dev->stats.tx_dropped++;
+		kfree_skb(skb);
+	}
 	goto done;
 }
 
@@ -408,6 +464,22 @@ static int virtnet_close(struct net_device *dev)
 	return 0;
 }
 
+static int virtnet_set_tx_csum(struct net_device *dev, u32 data)
+{
+	struct virtnet_info *vi = netdev_priv(dev);
+	struct virtio_device *vdev = vi->vdev;
+
+	if (data && !virtio_has_feature(vdev, VIRTIO_NET_F_CSUM))
+		return -ENOSYS;
+
+	return ethtool_op_set_tx_hw_csum(dev, data);
+}
+
+static struct ethtool_ops virtnet_ethtool_ops = {
+	.set_tx_csum = virtnet_set_tx_csum,
+	.set_sg = ethtool_op_set_sg,
+};
+
 static int virtnet_probe(struct virtio_device *vdev)
 {
 	int err;
@@ -427,6 +499,7 @@ static int virtnet_probe(struct virtio_device *vdev)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	dev->poll_controller = virtnet_netpoll;
 #endif
+	SET_ETHTOOL_OPS(dev, &virtnet_ethtool_ops);
 	SET_NETDEV_DEV(dev, &vdev->dev);
 
 	/* Do we support "hardware" checksums? */
@@ -462,11 +535,18 @@ static int virtnet_probe(struct virtio_device *vdev)
 	vi->dev = dev;
 	vi->vdev = vdev;
 	vdev->priv = vi;
+	vi->pages = NULL;
 
 	/* If they give us a callback when all buffers are done, we don't need
 	 * the timer. */
 	vi->free_in_tasklet = virtio_has_feature(vdev,VIRTIO_F_NOTIFY_ON_EMPTY);
 
+	/* If we can receive ANY GSO packets, we must allocate large ones. */
+	if (virtio_has_feature(vdev, VIRTIO_NET_F_GUEST_TSO4)
+	    || virtio_has_feature(vdev, VIRTIO_NET_F_GUEST_TSO6)
+	    || virtio_has_feature(vdev, VIRTIO_NET_F_GUEST_ECN))
+		vi->big_packets = true;
+
 	/* We expect two virtqueues, receive then send. */
 	vi->rvq = vdev->config->find_vq(vdev, 0, skb_recv_done);
 	if (IS_ERR(vi->rvq)) {
@@ -541,6 +621,10 @@ static void virtnet_remove(struct virtio_device *vdev)
 	vdev->config->del_vq(vi->svq);
 	vdev->config->del_vq(vi->rvq);
 	unregister_netdev(vi->dev);
+
+	while (vi->pages)
+		__free_pages(get_a_page(vi, GFP_KERNEL), 0);
+
 	free_netdev(vi->dev);
 }
 
@@ -553,7 +637,9 @@ static unsigned int features[] = {
 	VIRTIO_NET_F_CSUM, VIRTIO_NET_F_GUEST_CSUM,
 	VIRTIO_NET_F_GSO, VIRTIO_NET_F_MAC,
 	VIRTIO_NET_F_HOST_TSO4, VIRTIO_NET_F_HOST_UFO, VIRTIO_NET_F_HOST_TSO6,
-	VIRTIO_NET_F_HOST_ECN, VIRTIO_F_NOTIFY_ON_EMPTY,
+	VIRTIO_NET_F_HOST_ECN, VIRTIO_NET_F_GUEST_TSO4, VIRTIO_NET_F_GUEST_TSO6,
+	VIRTIO_NET_F_GUEST_ECN, /* We don't yet handle UFO input. */
+	VIRTIO_F_NOTIFY_ON_EMPTY,
 };
 
 static struct virtio_driver virtio_net = {
diff --git a/drivers/net/wireless/ath5k/base.c b/drivers/net/wireless/ath5k/base.c
index 217d506527a..d9769c52734 100644
--- a/drivers/net/wireless/ath5k/base.c
+++ b/drivers/net/wireless/ath5k/base.c
@@ -1166,7 +1166,7 @@ ath5k_rxbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 		bf->skb = skb;
 		bf->skbaddr = pci_map_single(sc->pdev,
 			skb->data, sc->rxbufsize, PCI_DMA_FROMDEVICE);
-		if (unlikely(pci_dma_mapping_error(bf->skbaddr))) {
+		if (unlikely(pci_dma_mapping_error(sc->pdev, bf->skbaddr))) {
 			ATH5K_ERR(sc, "%s: DMA mapping failed\n", __func__);
 			dev_kfree_skb(skb);
 			bf->skb = NULL;
@@ -1918,7 +1918,7 @@ ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 	ATH5K_DBG(sc, ATH5K_DEBUG_BEACON, "skb %p [data %p len %u] "
 			"skbaddr %llx\n", skb, skb->data, skb->len,
 			(unsigned long long)bf->skbaddr);
-	if (pci_dma_mapping_error(bf->skbaddr)) {
+	if (pci_dma_mapping_error(sc->pdev, bf->skbaddr)) {
 		ATH5K_ERR(sc, "beacon DMA mapping failed\n");
 		return -EIO;
 	}
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 3a7a11a75fb..1d7ec312934 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -4,7 +4,7 @@ config OF_DEVICE
 
 config OF_GPIO
 	def_bool y
-	depends on OF && PPC_OF && HAVE_GPIO_LIB
+	depends on OF && PPC_OF && GPIOLIB
 	help
 	  OpenFirmware GPIO accessors
 
diff --git a/drivers/of/of_i2c.c b/drivers/of/of_i2c.c
index 5c015d310d4..344e1b03dd8 100644
--- a/drivers/of/of_i2c.c
+++ b/drivers/of/of_i2c.c
@@ -91,8 +91,6 @@ void of_register_i2c_devices(struct i2c_adapter *adap,
 		}
 
 		info.irq = irq_of_parse_and_map(node, 0);
-		if (info.irq == NO_IRQ)
-			info.irq = -1;
 
 		if (of_find_i2c_driver(node, &info) < 0) {
 			irq_dispose_mapping(info.irq);
diff --git a/drivers/parport/ieee1284.c b/drivers/parport/ieee1284.c
index 0338b091267..e97059415ab 100644
--- a/drivers/parport/ieee1284.c
+++ b/drivers/parport/ieee1284.c
@@ -199,8 +199,6 @@ int parport_wait_peripheral(struct parport *port,
 	/* 40ms of slow polling. */
 	deadline = jiffies + msecs_to_jiffies(40);
 	while (time_before (jiffies, deadline)) {
-		int ret;
-
 		if (signal_pending (current))
 			return -EINTR;
 
diff --git a/drivers/parport/parport_ax88796.c b/drivers/parport/parport_ax88796.c
index 4ec220b2eae..6938d2e9f18 100644
--- a/drivers/parport/parport_ax88796.c
+++ b/drivers/parport/parport_ax88796.c
@@ -406,6 +406,8 @@ static int parport_ax88796_resume(struct platform_device *dev)
 #define parport_ax88796_resume  NULL
 #endif
 
+MODULE_ALIAS("platform:ax88796-pp");
+
 static struct platform_driver axdrv = {
 	.driver		= {
 		.name	= "ax88796-pp",
diff --git a/drivers/parport/parport_cs.c b/drivers/parport/parport_cs.c
index 802a81d4736..00e1d9620f7 100644
--- a/drivers/parport/parport_cs.c
+++ b/drivers/parport/parport_cs.c
@@ -235,7 +235,7 @@ failed:
     
 ======================================================================*/
 
-void parport_cs_release(struct pcmcia_device *link)
+static void parport_cs_release(struct pcmcia_device *link)
 {
 	parport_info_t *info = link->priv;
 
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index e0c2a4584ec..8a846adf1dc 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -2867,7 +2867,7 @@ static struct parport_pc_pci {
 	 * and 840 locks up if you write 1 to bit 2! */
 	/* oxsemi_952 */		{ 1, { { 0, 1 }, } },
 	/* oxsemi_954 */		{ 1, { { 0, -1 }, } },
-	/* oxsemi_840 */		{ 1, { { 0, -1 }, } },
+	/* oxsemi_840 */		{ 1, { { 0, 1 }, } },
 	/* aks_0100 */                  { 1, { { 0, -1 }, } },
 	/* mobility_pp */		{ 1, { { 0, 1 }, } },
 	/* netmos_9705 */               { 1, { { 0, -1 }, } }, /* untested */
diff --git a/drivers/parport/procfs.c b/drivers/parport/procfs.c
index d950fc34320..554e11f9e1c 100644
--- a/drivers/parport/procfs.c
+++ b/drivers/parport/procfs.c
@@ -429,9 +429,6 @@ struct parport_default_sysctl_table
 	ctl_table dev_dir[2];
 };
 
-extern unsigned long parport_default_timeslice;
-extern int parport_default_spintime;
-
 static struct parport_default_sysctl_table
 parport_default_sysctl_table = {
 	.sysctl_header	= NULL,
diff --git a/drivers/pci/dmar.c b/drivers/pci/dmar.c
index f941f609dbf..8bf86ae2333 100644
--- a/drivers/pci/dmar.c
+++ b/drivers/pci/dmar.c
@@ -317,10 +317,8 @@ int __init dmar_table_init(void)
 		return -ENODEV;
 	}
 
-	if (list_empty(&dmar_rmrr_units)) {
+	if (list_empty(&dmar_rmrr_units))
 		printk(KERN_INFO PREFIX "No RMRR found\n");
-		return -ENODEV;
-	}
 
 	return 0;
 }
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index d00f0e0d845..e9c356236d2 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1040,7 +1040,7 @@ int pci_set_pcie_reset_state(struct pci_dev *dev, enum pcie_reset_state state)
  * @dev: PCI device to handle.
  * @state: PCI state from which device will issue PME#.
  */
-static bool pci_pme_capable(struct pci_dev *dev, pci_power_t state)
+bool pci_pme_capable(struct pci_dev *dev, pci_power_t state)
 {
 	if (!dev->pm_cap)
 		return false;
@@ -1123,17 +1123,10 @@ int pci_enable_wake(struct pci_dev *dev, pci_power_t state, int enable)
 }
 
 /**
- * pci_prepare_to_sleep - prepare PCI device for system-wide transition into a sleep state
- * @dev: Device to handle.
- *
- * Choose the power state appropriate for the device depending on whether
- * it can wake up the system and/or is power manageable by the platform
- * (PCI_D3hot is the default) and put the device into that state.
  */
-int pci_prepare_to_sleep(struct pci_dev *dev)
+pci_power_t pci_target_state(struct pci_dev *dev)
 {
 	pci_power_t target_state = PCI_D3hot;
-	int error;
 
 	if (platform_pci_power_manageable(dev)) {
 		/*
@@ -1160,7 +1153,7 @@ int pci_prepare_to_sleep(struct pci_dev *dev)
 		 * to generate PME#.
 		 */
 		if (!dev->pm_cap)
-			return -EIO;
+			return PCI_POWER_ERROR;
 
 		if (dev->pme_support) {
 			while (target_state
@@ -1169,6 +1162,25 @@ int pci_prepare_to_sleep(struct pci_dev *dev)
 		}
 	}
 
+	return target_state;
+}
+
+/**
+ * pci_prepare_to_sleep - prepare PCI device for system-wide transition into a sleep state
+ * @dev: Device to handle.
+ *
+ * Choose the power state appropriate for the device depending on whether
+ * it can wake up the system and/or is power manageable by the platform
+ * (PCI_D3hot is the default) and put the device into that state.
+ */
+int pci_prepare_to_sleep(struct pci_dev *dev)
+{
+	pci_power_t target_state = pci_target_state(dev);
+	int error;
+
+	if (target_state == PCI_POWER_ERROR)
+		return -EIO;
+
 	pci_enable_wake(dev, target_state, true);
 
 	error = pci_set_power_state(dev, target_state);
@@ -1918,7 +1930,9 @@ EXPORT_SYMBOL(pci_select_bars);
 EXPORT_SYMBOL(pci_set_power_state);
 EXPORT_SYMBOL(pci_save_state);
 EXPORT_SYMBOL(pci_restore_state);
+EXPORT_SYMBOL(pci_pme_capable);
 EXPORT_SYMBOL(pci_enable_wake);
+EXPORT_SYMBOL(pci_target_state);
 EXPORT_SYMBOL(pci_prepare_to_sleep);
 EXPORT_SYMBOL(pci_back_from_sleep);
 EXPORT_SYMBOL_GPL(pci_set_pcie_reset_state);
diff --git a/drivers/pci/proc.c b/drivers/pci/proc.c
index 4400dffbd93..e1098c302c4 100644
--- a/drivers/pci/proc.c
+++ b/drivers/pci/proc.c
@@ -88,7 +88,7 @@ proc_bus_pci_read(struct file *file, char __user *buf, size_t nbytes, loff_t *pp
 	if ((pos & 3) && cnt > 2) {
 		unsigned short val;
 		pci_user_read_config_word(dev, pos, &val);
-		__put_user(cpu_to_le16(val), (unsigned short __user *) buf);
+		__put_user(cpu_to_le16(val), (__le16 __user *) buf);
 		buf += 2;
 		pos += 2;
 		cnt -= 2;
@@ -97,7 +97,7 @@ proc_bus_pci_read(struct file *file, char __user *buf, size_t nbytes, loff_t *pp
 	while (cnt >= 4) {
 		unsigned int val;
 		pci_user_read_config_dword(dev, pos, &val);
-		__put_user(cpu_to_le32(val), (unsigned int __user *) buf);
+		__put_user(cpu_to_le32(val), (__le32 __user *) buf);
 		buf += 4;
 		pos += 4;
 		cnt -= 4;
@@ -106,7 +106,7 @@ proc_bus_pci_read(struct file *file, char __user *buf, size_t nbytes, loff_t *pp
 	if (cnt >= 2) {
 		unsigned short val;
 		pci_user_read_config_word(dev, pos, &val);
-		__put_user(cpu_to_le16(val), (unsigned short __user *) buf);
+		__put_user(cpu_to_le16(val), (__le16 __user *) buf);
 		buf += 2;
 		pos += 2;
 		cnt -= 2;
@@ -156,8 +156,8 @@ proc_bus_pci_write(struct file *file, const char __user *buf, size_t nbytes, lof
 	}
 
 	if ((pos & 3) && cnt > 2) {
-		unsigned short val;
-		__get_user(val, (unsigned short __user *) buf);
+		__le16 val;
+		__get_user(val, (__le16 __user *) buf);
 		pci_user_write_config_word(dev, pos, le16_to_cpu(val));
 		buf += 2;
 		pos += 2;
@@ -165,8 +165,8 @@ proc_bus_pci_write(struct file *file, const char __user *buf, size_t nbytes, lof
 	}
 
 	while (cnt >= 4) {
-		unsigned int val;
-		__get_user(val, (unsigned int __user *) buf);
+		__le32 val;
+		__get_user(val, (__le32 __user *) buf);
 		pci_user_write_config_dword(dev, pos, le32_to_cpu(val));
 		buf += 4;
 		pos += 4;
@@ -174,8 +174,8 @@ proc_bus_pci_write(struct file *file, const char __user *buf, size_t nbytes, lof
 	}
 
 	if (cnt >= 2) {
-		unsigned short val;
-		__get_user(val, (unsigned short __user *) buf);
+		__le16 val;
+		__get_user(val, (__le16 __user *) buf);
 		pci_user_write_config_word(dev, pos, le16_to_cpu(val));
 		buf += 2;
 		pos += 2;
diff --git a/drivers/pcmcia/electra_cf.c b/drivers/pcmcia/electra_cf.c
index c21f9a9c3e3..a34284b1482 100644
--- a/drivers/pcmcia/electra_cf.c
+++ b/drivers/pcmcia/electra_cf.c
@@ -28,6 +28,7 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/mm.h>
 #include <linux/vmalloc.h>
 #include <linux/of_platform.h>
 
diff --git a/drivers/pnp/base.h b/drivers/pnp/base.h
index e3fa9a2d9a3..9fd7bb9b7dc 100644
--- a/drivers/pnp/base.h
+++ b/drivers/pnp/base.h
@@ -19,7 +19,6 @@ struct pnp_id *pnp_add_id(struct pnp_dev *dev, char *id);
 int pnp_interface_attach_device(struct pnp_dev *dev);
 
 int pnp_add_card(struct pnp_card *card);
-struct pnp_id *pnp_add_card_id(struct pnp_card *card, char *id);
 void pnp_remove_card(struct pnp_card *card);
 int pnp_add_card_device(struct pnp_card *card, struct pnp_dev *dev);
 void pnp_remove_card_device(struct pnp_dev *dev);
diff --git a/drivers/pnp/card.c b/drivers/pnp/card.c
index a762a417673..e75b060daa9 100644
--- a/drivers/pnp/card.c
+++ b/drivers/pnp/card.c
@@ -8,6 +8,7 @@
 #include <linux/ctype.h>
 #include <linux/slab.h>
 #include <linux/pnp.h>
+#include <linux/dma-mapping.h>
 #include "base.h"
 
 LIST_HEAD(pnp_cards);
@@ -101,7 +102,7 @@ static int card_probe(struct pnp_card *card, struct pnp_card_driver *drv)
  * @id: pointer to a pnp_id structure
  * @card: pointer to the desired card
  */
-struct pnp_id *pnp_add_card_id(struct pnp_card *card, char *id)
+static struct pnp_id *pnp_add_card_id(struct pnp_card *card, char *id)
 {
 	struct pnp_id *dev_id, *ptr;
 
@@ -167,6 +168,9 @@ struct pnp_card *pnp_alloc_card(struct pnp_protocol *protocol, int id, char *pnp
 	sprintf(card->dev.bus_id, "%02x:%02x", card->protocol->number,
 		card->number);
 
+	card->dev.coherent_dma_mask = DMA_24BIT_MASK;
+	card->dev.dma_mask = &card->dev.coherent_dma_mask;
+
 	dev_id = pnp_add_card_id(card, pnpid);
 	if (!dev_id) {
 		kfree(card);
diff --git a/drivers/pnp/quirks.c b/drivers/pnp/quirks.c
index 55f55ed72dc..0bdf9b8a5e5 100644
--- a/drivers/pnp/quirks.c
+++ b/drivers/pnp/quirks.c
@@ -245,15 +245,17 @@ static void quirk_system_pci_resources(struct pnp_dev *dev)
 	 */
 	for_each_pci_dev(pdev) {
 		for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
-			if (!(pci_resource_flags(pdev, i) & IORESOURCE_MEM) ||
-			    pci_resource_len(pdev, i) == 0)
+			unsigned int type;
+
+			type = pci_resource_flags(pdev, i) &
+					(IORESOURCE_IO | IORESOURCE_MEM);
+			if (!type || pci_resource_len(pdev, i) == 0)
 				continue;
 
 			pci_start = pci_resource_start(pdev, i);
 			pci_end = pci_resource_end(pdev, i);
 			for (j = 0;
-			     (res = pnp_get_resource(dev, IORESOURCE_MEM, j));
-			     j++) {
+			     (res = pnp_get_resource(dev, type, j)); j++) {
 				if (res->start == 0 && res->end == 0)
 					continue;
 
@@ -283,9 +285,10 @@ static void quirk_system_pci_resources(struct pnp_dev *dev)
 				 * the PCI region, and that might prevent a PCI
 				 * driver from requesting its resources.
 				 */
-				dev_warn(&dev->dev, "mem resource "
+				dev_warn(&dev->dev, "%s resource "
 					"(0x%llx-0x%llx) overlaps %s BAR %d "
 					"(0x%llx-0x%llx), disabling\n",
+					pnp_resource_type_name(res),
 					(unsigned long long) pnp_start,
 					(unsigned long long) pnp_end,
 					pci_name(pdev), i,
diff --git a/drivers/power/ds2760_battery.c b/drivers/power/ds2760_battery.c
index 71be36f1870..308ddb201b6 100644
--- a/drivers/power/ds2760_battery.c
+++ b/drivers/power/ds2760_battery.c
@@ -433,6 +433,8 @@ static int ds2760_battery_resume(struct platform_device *pdev)
 
 #endif /* CONFIG_PM */
 
+MODULE_ALIAS("platform:ds2760-battery");
+
 static struct platform_driver ds2760_battery_driver = {
 	.driver = {
 		.name = "ds2760-battery",
diff --git a/drivers/power/pda_power.c b/drivers/power/pda_power.c
index 82810b7bff9..0471ec743ab 100644
--- a/drivers/power/pda_power.c
+++ b/drivers/power/pda_power.c
@@ -362,6 +362,8 @@ static int pda_power_resume(struct platform_device *pdev)
 #define pda_power_resume NULL
 #endif /* CONFIG_PM */
 
+MODULE_ALIAS("platform:pda-power");
+
 static struct platform_driver pda_power_pdrv = {
 	.driver = {
 		.name = "pda-power",
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index fc85bf2e4a9..90ab7382540 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -273,6 +273,25 @@ comment "SPI RTC drivers"
 
 if SPI_MASTER
 
+config RTC_DRV_M41T94
+	tristate "ST M41T94"
+	help
+	  If you say yes here you will get support for the
+	  ST M41T94 SPI RTC chip.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-m41t94.
+
+config RTC_DRV_DS1305
+	tristate "Dallas/Maxim DS1305/DS1306"
+	help
+	  Select this driver to get support for the Dallas/Maxim DS1305
+	  and DS1306 real time clock chips.  These support a trickle
+	  charger, alarms, and NVRAM in addition to the clock.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-ds1305.
+
 config RTC_DRV_MAX6902
 	tristate "Maxim MAX6902"
 	help
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index b5d9d67df88..18622ef84ca 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_RTC_DRV_BFIN)	+= rtc-bfin.o
 obj-$(CONFIG_RTC_DRV_CMOS)	+= rtc-cmos.o
 obj-$(CONFIG_RTC_DRV_DS1216)	+= rtc-ds1216.o
 obj-$(CONFIG_RTC_DRV_DS1302)	+= rtc-ds1302.o
+obj-$(CONFIG_RTC_DRV_DS1305)	+= rtc-ds1305.o
 obj-$(CONFIG_RTC_DRV_DS1307)	+= rtc-ds1307.o
 obj-$(CONFIG_RTC_DRV_DS1374)	+= rtc-ds1374.o
 obj-$(CONFIG_RTC_DRV_DS1511)	+= rtc-ds1511.o
@@ -34,6 +35,7 @@ obj-$(CONFIG_RTC_DRV_EP93XX)	+= rtc-ep93xx.o
 obj-$(CONFIG_RTC_DRV_FM3130)	+= rtc-fm3130.o
 obj-$(CONFIG_RTC_DRV_ISL1208)	+= rtc-isl1208.o
 obj-$(CONFIG_RTC_DRV_M41T80)	+= rtc-m41t80.o
+obj-$(CONFIG_RTC_DRV_M41T94)	+= rtc-m41t94.o
 obj-$(CONFIG_RTC_DRV_M48T59)	+= rtc-m48t59.o
 obj-$(CONFIG_RTC_DRV_M48T86)	+= rtc-m48t86.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index 9c3db934cc2..cd32d05db77 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -171,8 +171,10 @@ static int at91_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 		| BIN2BCD(tm.tm_mday) << 24
 		| AT91_RTC_DATEEN | AT91_RTC_MTHEN);
 
-	if (alrm->enabled)
+	if (alrm->enabled) {
+		at91_sys_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
 		at91_sys_write(AT91_RTC_IER, AT91_RTC_ALARM);
+	}
 
 	pr_debug("%s(): %4d-%02d-%02d %02d:%02d:%02d\n", __func__,
 		at91_alarm_year, tm.tm_mon, tm.tm_mday, tm.tm_hour,
@@ -191,28 +193,22 @@ static int at91_rtc_ioctl(struct device *dev, unsigned int cmd,
 
 	pr_debug("%s(): cmd=%08x, arg=%08lx.\n", __func__, cmd, arg);
 
+	/* important:  scrub old status before enabling IRQs */
 	switch (cmd) {
 	case RTC_AIE_OFF:	/* alarm off */
 		at91_sys_write(AT91_RTC_IDR, AT91_RTC_ALARM);
 		break;
 	case RTC_AIE_ON:	/* alarm on */
+		at91_sys_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
 		at91_sys_write(AT91_RTC_IER, AT91_RTC_ALARM);
 		break;
 	case RTC_UIE_OFF:	/* update off */
-	case RTC_PIE_OFF:	/* periodic off */
 		at91_sys_write(AT91_RTC_IDR, AT91_RTC_SECEV);
 		break;
 	case RTC_UIE_ON:	/* update on */
-	case RTC_PIE_ON:	/* periodic on */
+		at91_sys_write(AT91_RTC_SCCR, AT91_RTC_SECEV);
 		at91_sys_write(AT91_RTC_IER, AT91_RTC_SECEV);
 		break;
-	case RTC_IRQP_READ:	/* read periodic alarm frequency */
-		ret = put_user(AT91_RTC_FREQ, (unsigned long *) arg);
-		break;
-	case RTC_IRQP_SET:	/* set periodic alarm frequency */
-		if (arg != AT91_RTC_FREQ)
-			ret = -EINVAL;
-		break;
 	default:
 		ret = -ENOIOCTLCMD;
 		break;
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index d7bb9bac71d..6ea349aba3b 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -36,25 +36,9 @@
 #include <linux/platform_device.h>
 #include <linux/mod_devicetable.h>
 
-#ifdef CONFIG_HPET_EMULATE_RTC
-#include <asm/hpet.h>
-#endif
-
 /* this is for "generic access to PC-style RTC" using CMOS_READ/CMOS_WRITE */
 #include <asm-generic/rtc.h>
 
-#ifndef CONFIG_HPET_EMULATE_RTC
-#define is_hpet_enabled()			0
-#define hpet_set_alarm_time(hrs, min, sec) 	do { } while (0)
-#define hpet_set_periodic_freq(arg) 		0
-#define hpet_mask_rtc_irq_bit(arg) 		do { } while (0)
-#define hpet_set_rtc_irq_bit(arg) 		do { } while (0)
-#define hpet_rtc_timer_init() 			do { } while (0)
-#define hpet_register_irq_handler(h) 		0
-#define hpet_unregister_irq_handler(h)		do { } while (0)
-extern irqreturn_t hpet_rtc_interrupt(int irq, void *dev_id);
-#endif
-
 struct cmos_rtc {
 	struct rtc_device	*rtc;
 	struct device		*dev;
@@ -93,6 +77,72 @@ static inline int is_intr(u8 rtc_intr)
 
 /*----------------------------------------------------------------*/
 
+/* Much modern x86 hardware has HPETs (10+ MHz timers) which, because
+ * many BIOS programmers don't set up "sane mode" IRQ routing, are mostly
+ * used in a broken "legacy replacement" mode.  The breakage includes
+ * HPET #1 hijacking the IRQ for this RTC, and being unavailable for
+ * other (better) use.
+ *
+ * When that broken mode is in use, platform glue provides a partial
+ * emulation of hardware RTC IRQ facilities using HPET #1.  We don't
+ * want to use HPET for anything except those IRQs though...
+ */
+#ifdef CONFIG_HPET_EMULATE_RTC
+#include <asm/hpet.h>
+#else
+
+static inline int is_hpet_enabled(void)
+{
+	return 0;
+}
+
+static inline int hpet_mask_rtc_irq_bit(unsigned long mask)
+{
+	return 0;
+}
+
+static inline int hpet_set_rtc_irq_bit(unsigned long mask)
+{
+	return 0;
+}
+
+static inline int
+hpet_set_alarm_time(unsigned char hrs, unsigned char min, unsigned char sec)
+{
+	return 0;
+}
+
+static inline int hpet_set_periodic_freq(unsigned long freq)
+{
+	return 0;
+}
+
+static inline int hpet_rtc_dropped_irq(void)
+{
+	return 0;
+}
+
+static inline int hpet_rtc_timer_init(void)
+{
+	return 0;
+}
+
+extern irq_handler_t hpet_rtc_interrupt;
+
+static inline int hpet_register_irq_handler(irq_handler_t handler)
+{
+	return 0;
+}
+
+static inline int hpet_unregister_irq_handler(irq_handler_t handler)
+{
+	return 0;
+}
+
+#endif
+
+/*----------------------------------------------------------------*/
+
 static int cmos_read_time(struct device *dev, struct rtc_time *t)
 {
 	/* REVISIT:  if the clock has a "century" register, use
@@ -185,11 +235,56 @@ static int cmos_read_alarm(struct device *dev, struct rtc_wkalrm *t)
 	return 0;
 }
 
+static void cmos_checkintr(struct cmos_rtc *cmos, unsigned char rtc_control)
+{
+	unsigned char	rtc_intr;
+
+	/* NOTE after changing RTC_xIE bits we always read INTR_FLAGS;
+	 * allegedly some older rtcs need that to handle irqs properly
+	 */
+	rtc_intr = CMOS_READ(RTC_INTR_FLAGS);
+
+	if (is_hpet_enabled())
+		return;
+
+	rtc_intr &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
+	if (is_intr(rtc_intr))
+		rtc_update_irq(cmos->rtc, 1, rtc_intr);
+}
+
+static void cmos_irq_enable(struct cmos_rtc *cmos, unsigned char mask)
+{
+	unsigned char	rtc_control;
+
+	/* flush any pending IRQ status, notably for update irqs,
+	 * before we enable new IRQs
+	 */
+	rtc_control = CMOS_READ(RTC_CONTROL);
+	cmos_checkintr(cmos, rtc_control);
+
+	rtc_control |= mask;
+	CMOS_WRITE(rtc_control, RTC_CONTROL);
+	hpet_set_rtc_irq_bit(mask);
+
+	cmos_checkintr(cmos, rtc_control);
+}
+
+static void cmos_irq_disable(struct cmos_rtc *cmos, unsigned char mask)
+{
+	unsigned char	rtc_control;
+
+	rtc_control = CMOS_READ(RTC_CONTROL);
+	rtc_control &= ~mask;
+	CMOS_WRITE(rtc_control, RTC_CONTROL);
+	hpet_mask_rtc_irq_bit(mask);
+
+	cmos_checkintr(cmos, rtc_control);
+}
+
 static int cmos_set_alarm(struct device *dev, struct rtc_wkalrm *t)
 {
 	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
 	unsigned char	mon, mday, hrs, min, sec;
-	unsigned char	rtc_control, rtc_intr;
 
 	if (!is_valid_irq(cmos->irq))
 		return -EIO;
@@ -213,17 +308,10 @@ static int cmos_set_alarm(struct device *dev, struct rtc_wkalrm *t)
 	sec = t->time.tm_sec;
 	sec = (sec < 60) ? BIN2BCD(sec) : 0xff;
 
-	hpet_set_alarm_time(t->time.tm_hour, t->time.tm_min, t->time.tm_sec);
 	spin_lock_irq(&rtc_lock);
 
 	/* next rtc irq must not be from previous alarm setting */
-	rtc_control = CMOS_READ(RTC_CONTROL);
-	rtc_control &= ~RTC_AIE;
-	CMOS_WRITE(rtc_control, RTC_CONTROL);
-	rtc_intr = CMOS_READ(RTC_INTR_FLAGS);
-	rtc_intr &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
-	if (is_intr(rtc_intr))
-		rtc_update_irq(cmos->rtc, 1, rtc_intr);
+	cmos_irq_disable(cmos, RTC_AIE);
 
 	/* update alarm */
 	CMOS_WRITE(hrs, RTC_HOURS_ALARM);
@@ -237,14 +325,13 @@ static int cmos_set_alarm(struct device *dev, struct rtc_wkalrm *t)
 			CMOS_WRITE(mon, cmos->mon_alrm);
 	}
 
-	if (t->enabled) {
-		rtc_control |= RTC_AIE;
-		CMOS_WRITE(rtc_control, RTC_CONTROL);
-		rtc_intr = CMOS_READ(RTC_INTR_FLAGS);
-		rtc_intr &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
-		if (is_intr(rtc_intr))
-			rtc_update_irq(cmos->rtc, 1, rtc_intr);
-	}
+	/* FIXME the HPET alarm glue currently ignores day_alrm
+	 * and mon_alrm ...
+	 */
+	hpet_set_alarm_time(t->time.tm_hour, t->time.tm_min, t->time.tm_sec);
+
+	if (t->enabled)
+		cmos_irq_enable(cmos, RTC_AIE);
 
 	spin_unlock_irq(&rtc_lock);
 
@@ -267,8 +354,8 @@ static int cmos_irq_set_freq(struct device *dev, int freq)
 	f = 16 - f;
 
 	spin_lock_irqsave(&rtc_lock, flags);
-	if (!hpet_set_periodic_freq(freq))
-		CMOS_WRITE(RTC_REF_CLCK_32KHZ | f, RTC_FREQ_SELECT);
+	hpet_set_periodic_freq(freq);
+	CMOS_WRITE(RTC_REF_CLCK_32KHZ | f, RTC_FREQ_SELECT);
 	spin_unlock_irqrestore(&rtc_lock, flags);
 
 	return 0;
@@ -277,26 +364,17 @@ static int cmos_irq_set_freq(struct device *dev, int freq)
 static int cmos_irq_set_state(struct device *dev, int enabled)
 {
 	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
-	unsigned char	rtc_control, rtc_intr;
 	unsigned long	flags;
 
 	if (!is_valid_irq(cmos->irq))
 		return -ENXIO;
 
 	spin_lock_irqsave(&rtc_lock, flags);
-	rtc_control = CMOS_READ(RTC_CONTROL);
 
 	if (enabled)
-		rtc_control |= RTC_PIE;
+		cmos_irq_enable(cmos, RTC_PIE);
 	else
-		rtc_control &= ~RTC_PIE;
-
-	CMOS_WRITE(rtc_control, RTC_CONTROL);
-
-	rtc_intr = CMOS_READ(RTC_INTR_FLAGS);
-	rtc_intr &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
-	if (is_intr(rtc_intr))
-		rtc_update_irq(cmos->rtc, 1, rtc_intr);
+		cmos_irq_disable(cmos, RTC_PIE);
 
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return 0;
@@ -308,7 +386,6 @@ static int
 cmos_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
 {
 	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
-	unsigned char	rtc_control, rtc_intr;
 	unsigned long	flags;
 
 	switch (cmd) {
@@ -316,51 +393,29 @@ cmos_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
 	case RTC_AIE_ON:
 	case RTC_UIE_OFF:
 	case RTC_UIE_ON:
-	case RTC_PIE_OFF:
-	case RTC_PIE_ON:
 		if (!is_valid_irq(cmos->irq))
 			return -EINVAL;
 		break;
+	/* PIE ON/OFF is handled by cmos_irq_set_state() */
 	default:
 		return -ENOIOCTLCMD;
 	}
 
 	spin_lock_irqsave(&rtc_lock, flags);
-	rtc_control = CMOS_READ(RTC_CONTROL);
 	switch (cmd) {
 	case RTC_AIE_OFF:	/* alarm off */
-		rtc_control &= ~RTC_AIE;
-		hpet_mask_rtc_irq_bit(RTC_AIE);
+		cmos_irq_disable(cmos, RTC_AIE);
 		break;
 	case RTC_AIE_ON:	/* alarm on */
-		rtc_control |= RTC_AIE;
-		hpet_set_rtc_irq_bit(RTC_AIE);
+		cmos_irq_enable(cmos, RTC_AIE);
 		break;
 	case RTC_UIE_OFF:	/* update off */
-		rtc_control &= ~RTC_UIE;
-		hpet_mask_rtc_irq_bit(RTC_UIE);
+		cmos_irq_disable(cmos, RTC_UIE);
 		break;
 	case RTC_UIE_ON:	/* update on */
-		rtc_control |= RTC_UIE;
-		hpet_set_rtc_irq_bit(RTC_UIE);
-		break;
-	case RTC_PIE_OFF:	/* periodic off */
-		rtc_control &= ~RTC_PIE;
-		hpet_mask_rtc_irq_bit(RTC_PIE);
-		break;
-	case RTC_PIE_ON:	/* periodic on */
-		rtc_control |= RTC_PIE;
-		hpet_set_rtc_irq_bit(RTC_PIE);
+		cmos_irq_enable(cmos, RTC_UIE);
 		break;
 	}
-	if (!is_hpet_enabled())
-		CMOS_WRITE(rtc_control, RTC_CONTROL);
-
-	rtc_intr = CMOS_READ(RTC_INTR_FLAGS);
-	rtc_intr &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
-	if (is_intr(rtc_intr))
-		rtc_update_irq(cmos->rtc, 1, rtc_intr);
-
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return 0;
 }
@@ -502,27 +557,29 @@ static irqreturn_t cmos_interrupt(int irq, void *p)
 	u8		rtc_control;
 
 	spin_lock(&rtc_lock);
-	/*
-	 * In this case it is HPET RTC interrupt handler
-	 * calling us, with the interrupt information
-	 * passed as arg1, instead of irq.
+
+	/* When the HPET interrupt handler calls us, the interrupt
+	 * status is passed as arg1 instead of the irq number.  But
+	 * always clear irq status, even when HPET is in the way.
+	 *
+	 * Note that HPET and RTC are almost certainly out of phase,
+	 * giving different IRQ status ...
 	 */
+	irqstat = CMOS_READ(RTC_INTR_FLAGS);
+	rtc_control = CMOS_READ(RTC_CONTROL);
 	if (is_hpet_enabled())
 		irqstat = (unsigned long)irq & 0xF0;
-	else {
-		irqstat = CMOS_READ(RTC_INTR_FLAGS);
-		rtc_control = CMOS_READ(RTC_CONTROL);
-		irqstat &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
-	}
+	irqstat &= (rtc_control & RTC_IRQMASK) | RTC_IRQF;
 
 	/* All Linux RTC alarms should be treated as if they were oneshot.
 	 * Similar code may be needed in system wakeup paths, in case the
 	 * alarm woke the system.
 	 */
 	if (irqstat & RTC_AIE) {
-		rtc_control = CMOS_READ(RTC_CONTROL);
 		rtc_control &= ~RTC_AIE;
 		CMOS_WRITE(rtc_control, RTC_CONTROL);
+		hpet_mask_rtc_irq_bit(RTC_AIE);
+
 		CMOS_READ(RTC_INTR_FLAGS);
 	}
 	spin_unlock(&rtc_lock);
@@ -629,18 +686,13 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	 * do something about other clock frequencies.
 	 */
 	cmos_rtc.rtc->irq_freq = 1024;
-	if (!hpet_set_periodic_freq(cmos_rtc.rtc->irq_freq))
-		CMOS_WRITE(RTC_REF_CLCK_32KHZ | 0x06, RTC_FREQ_SELECT);
+	hpet_set_periodic_freq(cmos_rtc.rtc->irq_freq);
+	CMOS_WRITE(RTC_REF_CLCK_32KHZ | 0x06, RTC_FREQ_SELECT);
+
+	/* disable irqs */
+	cmos_irq_disable(&cmos_rtc, RTC_PIE | RTC_AIE | RTC_UIE);
 
-	/* disable irqs.
-	 *
-	 * NOTE after changing RTC_xIE bits we always read INTR_FLAGS;
-	 * allegedly some older rtcs need that to handle irqs properly
-	 */
 	rtc_control = CMOS_READ(RTC_CONTROL);
-	rtc_control &= ~(RTC_PIE | RTC_AIE | RTC_UIE);
-	CMOS_WRITE(rtc_control, RTC_CONTROL);
-	CMOS_READ(RTC_INTR_FLAGS);
 
 	spin_unlock_irq(&rtc_lock);
 
@@ -687,7 +739,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 		goto cleanup2;
 	}
 
-	pr_info("%s: alarms up to one %s%s\n",
+	pr_info("%s: alarms up to one %s%s%s\n",
 			cmos_rtc.rtc->dev.bus_id,
 			is_valid_irq(rtc_irq)
 				?  (cmos_rtc.mon_alrm
@@ -695,8 +747,8 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 					: (cmos_rtc.day_alrm
 						? "month" : "day"))
 				: "no",
-			cmos_rtc.century ? ", y3k" : ""
-			);
+			cmos_rtc.century ? ", y3k" : "",
+			is_hpet_enabled() ? ", hpet irqs" : "");
 
 	return 0;
 
@@ -713,13 +765,8 @@ cleanup0:
 
 static void cmos_do_shutdown(void)
 {
-	unsigned char	rtc_control;
-
 	spin_lock_irq(&rtc_lock);
-	rtc_control = CMOS_READ(RTC_CONTROL);
-	rtc_control &= ~(RTC_PIE|RTC_AIE|RTC_UIE);
-	CMOS_WRITE(rtc_control, RTC_CONTROL);
-	CMOS_READ(RTC_INTR_FLAGS);
+	cmos_irq_disable(&cmos_rtc, RTC_IRQMASK);
 	spin_unlock_irq(&rtc_lock);
 }
 
@@ -760,17 +807,17 @@ static int cmos_suspend(struct device *dev, pm_message_t mesg)
 	spin_lock_irq(&rtc_lock);
 	cmos->suspend_ctrl = tmp = CMOS_READ(RTC_CONTROL);
 	if (tmp & (RTC_PIE|RTC_AIE|RTC_UIE)) {
-		unsigned char	irqstat;
+		unsigned char	mask;
 
 		if (do_wake)
-			tmp &= ~(RTC_PIE|RTC_UIE);
+			mask = RTC_IRQMASK & ~RTC_AIE;
 		else
-			tmp &= ~(RTC_PIE|RTC_AIE|RTC_UIE);
+			mask = RTC_IRQMASK;
+		tmp &= ~mask;
 		CMOS_WRITE(tmp, RTC_CONTROL);
-		irqstat = CMOS_READ(RTC_INTR_FLAGS);
-		irqstat &= (tmp & RTC_IRQMASK) | RTC_IRQF;
-		if (is_intr(irqstat))
-			rtc_update_irq(cmos->rtc, 1, irqstat);
+		hpet_mask_rtc_irq_bit(mask);
+
+		cmos_checkintr(cmos, tmp);
 	}
 	spin_unlock_irq(&rtc_lock);
 
@@ -796,7 +843,8 @@ static int cmos_resume(struct device *dev)
 	unsigned char	tmp = cmos->suspend_ctrl;
 
 	/* re-enable any irqs previously active */
-	if (tmp & (RTC_PIE|RTC_AIE|RTC_UIE)) {
+	if (tmp & RTC_IRQMASK) {
+		unsigned char	mask;
 
 		if (cmos->enabled_wake) {
 			if (cmos->wake_off)
@@ -807,18 +855,28 @@ static int cmos_resume(struct device *dev)
 		}
 
 		spin_lock_irq(&rtc_lock);
-		CMOS_WRITE(tmp, RTC_CONTROL);
-		tmp = CMOS_READ(RTC_INTR_FLAGS);
-		tmp &= (cmos->suspend_ctrl & RTC_IRQMASK) | RTC_IRQF;
-		if (is_intr(tmp))
-			rtc_update_irq(cmos->rtc, 1, tmp);
+		do {
+			CMOS_WRITE(tmp, RTC_CONTROL);
+			hpet_set_rtc_irq_bit(tmp & RTC_IRQMASK);
+
+			mask = CMOS_READ(RTC_INTR_FLAGS);
+			mask &= (tmp & RTC_IRQMASK) | RTC_IRQF;
+			if (!is_hpet_enabled() || !is_intr(mask))
+				break;
+
+			/* force one-shot behavior if HPET blocked
+			 * the wake alarm's irq
+			 */
+			rtc_update_irq(cmos->rtc, 1, mask);
+			tmp &= ~RTC_AIE;
+			hpet_mask_rtc_irq_bit(RTC_AIE);
+		} while (mask & RTC_AIE);
 		spin_unlock_irq(&rtc_lock);
 	}
 
 	pr_debug("%s: resume, ctrl %02x\n",
 			cmos_rtc.rtc->dev.bus_id,
-			cmos->suspend_ctrl);
-
+			tmp);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-dev.c b/drivers/rtc/rtc-dev.c
index 0114a78b7cb..0a870b7e5c3 100644
--- a/drivers/rtc/rtc-dev.c
+++ b/drivers/rtc/rtc-dev.c
@@ -209,7 +209,7 @@ static unsigned int rtc_dev_poll(struct file *file, poll_table *wait)
 	return (data != 0) ? (POLLIN | POLLRDNORM) : 0;
 }
 
-static int rtc_dev_ioctl(struct inode *inode, struct file *file,
+static long rtc_dev_ioctl(struct file *file,
 		unsigned int cmd, unsigned long arg)
 {
 	int err = 0;
@@ -219,6 +219,10 @@ static int rtc_dev_ioctl(struct inode *inode, struct file *file,
 	struct rtc_wkalrm alarm;
 	void __user *uarg = (void __user *) arg;
 
+	err = mutex_lock_interruptible(&rtc->ops_lock);
+	if (err)
+		return -EBUSY;
+
 	/* check that the calling task has appropriate permissions
 	 * for certain ioctls. doing this check here is useful
 	 * to avoid duplicate code in each driver.
@@ -227,26 +231,31 @@ static int rtc_dev_ioctl(struct inode *inode, struct file *file,
 	case RTC_EPOCH_SET:
 	case RTC_SET_TIME:
 		if (!capable(CAP_SYS_TIME))
-			return -EACCES;
+			err = -EACCES;
 		break;
 
 	case RTC_IRQP_SET:
 		if (arg > rtc->max_user_freq && !capable(CAP_SYS_RESOURCE))
-			return -EACCES;
+			err = -EACCES;
 		break;
 
 	case RTC_PIE_ON:
 		if (rtc->irq_freq > rtc->max_user_freq &&
 				!capable(CAP_SYS_RESOURCE))
-			return -EACCES;
+			err = -EACCES;
 		break;
 	}
 
+	if (err)
+		goto done;
+
 	/* try the driver's ioctl interface */
 	if (ops->ioctl) {
 		err = ops->ioctl(rtc->dev.parent, cmd, arg);
-		if (err != -ENOIOCTLCMD)
+		if (err != -ENOIOCTLCMD) {
+			mutex_unlock(&rtc->ops_lock);
 			return err;
+		}
 	}
 
 	/* if the driver does not provide the ioctl interface
@@ -265,15 +274,19 @@ static int rtc_dev_ioctl(struct inode *inode, struct file *file,
 
 	switch (cmd) {
 	case RTC_ALM_READ:
+		mutex_unlock(&rtc->ops_lock);
+
 		err = rtc_read_alarm(rtc, &alarm);
 		if (err < 0)
 			return err;
 
 		if (copy_to_user(uarg, &alarm.time, sizeof(tm)))
-			return -EFAULT;
-		break;
+			err = -EFAULT;
+		return err;
 
 	case RTC_ALM_SET:
+		mutex_unlock(&rtc->ops_lock);
+
 		if (copy_from_user(&alarm.time, uarg, sizeof(tm)))
 			return -EFAULT;
 
@@ -321,24 +334,26 @@ static int rtc_dev_ioctl(struct inode *inode, struct file *file,
 			}
 		}
 
-		err = rtc_set_alarm(rtc, &alarm);
-		break;
+		return rtc_set_alarm(rtc, &alarm);
 
 	case RTC_RD_TIME:
+		mutex_unlock(&rtc->ops_lock);
+
 		err = rtc_read_time(rtc, &tm);
 		if (err < 0)
 			return err;
 
 		if (copy_to_user(uarg, &tm, sizeof(tm)))
-			return -EFAULT;
-		break;
+			err = -EFAULT;
+		return err;
 
 	case RTC_SET_TIME:
+		mutex_unlock(&rtc->ops_lock);
+
 		if (copy_from_user(&tm, uarg, sizeof(tm)))
 			return -EFAULT;
 
-		err = rtc_set_time(rtc, &tm);
-		break;
+		return rtc_set_time(rtc, &tm);
 
 	case RTC_PIE_ON:
 		err = rtc_irq_set_state(rtc, NULL, 1);
@@ -376,34 +391,37 @@ static int rtc_dev_ioctl(struct inode *inode, struct file *file,
 		break;
 #endif
 	case RTC_WKALM_SET:
+		mutex_unlock(&rtc->ops_lock);
 		if (copy_from_user(&alarm, uarg, sizeof(alarm)))
 			return -EFAULT;
 
-		err = rtc_set_alarm(rtc, &alarm);
-		break;
+		return rtc_set_alarm(rtc, &alarm);
 
 	case RTC_WKALM_RD:
+		mutex_unlock(&rtc->ops_lock);
 		err = rtc_read_alarm(rtc, &alarm);
 		if (err < 0)
 			return err;
 
 		if (copy_to_user(uarg, &alarm, sizeof(alarm)))
-			return -EFAULT;
-		break;
+			err = -EFAULT;
+		return err;
 
 #ifdef CONFIG_RTC_INTF_DEV_UIE_EMUL
 	case RTC_UIE_OFF:
 		clear_uie(rtc);
-		return 0;
+		break;
 
 	case RTC_UIE_ON:
-		return set_uie(rtc);
+		err = set_uie(rtc);
 #endif
 	default:
 		err = -ENOTTY;
 		break;
 	}
 
+done:
+	mutex_unlock(&rtc->ops_lock);
 	return err;
 }
 
@@ -432,7 +450,7 @@ static const struct file_operations rtc_dev_fops = {
 	.llseek		= no_llseek,
 	.read		= rtc_dev_read,
 	.poll		= rtc_dev_poll,
-	.ioctl		= rtc_dev_ioctl,
+	.unlocked_ioctl	= rtc_dev_ioctl,
 	.open		= rtc_dev_open,
 	.release	= rtc_dev_release,
 	.fasync		= rtc_dev_fasync,
diff --git a/drivers/rtc/rtc-ds1305.c b/drivers/rtc/rtc-ds1305.c
new file mode 100644
index 00000000000..b91d02a3ace
--- /dev/null
+++ b/drivers/rtc/rtc-ds1305.c
@@ -0,0 +1,847 @@
+/*
+ * rtc-ds1305.c -- driver for DS1305 and DS1306 SPI RTC chips
+ *
+ * Copyright (C) 2008 David Brownell
+ *
+ * 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/init.h>
+#include <linux/bcd.h>
+#include <linux/rtc.h>
+#include <linux/workqueue.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi/ds1305.h>
+
+
+/*
+ * Registers ... mask DS1305_WRITE into register address to write,
+ * otherwise you're reading it.  All non-bitmask values are BCD.
+ */
+#define DS1305_WRITE		0x80
+
+
+/* RTC date/time ... the main special cases are that we:
+ *  - Need fancy "hours" encoding in 12hour mode
+ *  - Don't rely on the "day-of-week" field (or tm_wday)
+ *  - Are a 21st-century clock (2000 <= year < 2100)
+ */
+#define DS1305_RTC_LEN		7		/* bytes for RTC regs */
+
+#define DS1305_SEC		0x00		/* register addresses */
+#define DS1305_MIN		0x01
+#define DS1305_HOUR		0x02
+#	define DS1305_HR_12		0x40	/* set == 12 hr mode */
+#	define DS1305_HR_PM		0x20	/* set == PM (12hr mode) */
+#define DS1305_WDAY		0x03
+#define DS1305_MDAY		0x04
+#define DS1305_MON		0x05
+#define DS1305_YEAR		0x06
+
+
+/* The two alarms have only sec/min/hour/wday fields (ALM_LEN).
+ * DS1305_ALM_DISABLE disables a match field (some combos are bad).
+ *
+ * NOTE that since we don't use WDAY, we limit ourselves to alarms
+ * only one day into the future (vs potentially up to a week).
+ *
+ * NOTE ALSO that while we could generate once-a-second IRQs (UIE), we
+ * don't currently support them.  We'd either need to do it only when
+ * no alarm is pending (not the standard model), or to use the second
+ * alarm (implying that this is a DS1305 not DS1306, *and* that either
+ * it's wired up a second IRQ we know, or that INTCN is set)
+ */
+#define DS1305_ALM_LEN		4		/* bytes for ALM regs */
+#define DS1305_ALM_DISABLE	0x80
+
+#define DS1305_ALM0(r)		(0x07 + (r))	/* register addresses */
+#define DS1305_ALM1(r)		(0x0b + (r))
+
+
+/* three control registers */
+#define DS1305_CONTROL_LEN	3		/* bytes of control regs */
+
+#define DS1305_CONTROL		0x0f		/* register addresses */
+#	define DS1305_nEOSC		0x80	/* low enables oscillator */
+#	define DS1305_WP		0x40	/* write protect */
+#	define DS1305_INTCN		0x04	/* clear == only int0 used */
+#	define DS1306_1HZ		0x04	/* enable 1Hz output */
+#	define DS1305_AEI1		0x02	/* enable ALM1 IRQ */
+#	define DS1305_AEI0		0x01	/* enable ALM0 IRQ */
+#define DS1305_STATUS		0x10
+/* status has just AEIx bits, mirrored as IRQFx */
+#define DS1305_TRICKLE		0x11
+/* trickle bits are defined in <linux/spi/ds1305.h> */
+
+/* a bunch of NVRAM */
+#define DS1305_NVRAM_LEN	96		/* bytes of NVRAM */
+
+#define DS1305_NVRAM		0x20		/* register addresses */
+
+
+struct ds1305 {
+	struct spi_device	*spi;
+	struct rtc_device	*rtc;
+
+	struct work_struct	work;
+
+	unsigned long		flags;
+#define FLAG_EXITING	0
+
+	bool			hr12;
+	u8			ctrl[DS1305_CONTROL_LEN];
+};
+
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Utilities ...  tolerate 12-hour AM/PM notation in case of non-Linux
+ * software (like a bootloader) which may require it.
+ */
+
+static unsigned bcd2hour(u8 bcd)
+{
+	if (bcd & DS1305_HR_12) {
+		unsigned	hour = 0;
+
+		bcd &= ~DS1305_HR_12;
+		if (bcd & DS1305_HR_PM) {
+			hour = 12;
+			bcd &= ~DS1305_HR_PM;
+		}
+		hour += BCD2BIN(bcd);
+		return hour - 1;
+	}
+	return BCD2BIN(bcd);
+}
+
+static u8 hour2bcd(bool hr12, int hour)
+{
+	if (hr12) {
+		hour++;
+		if (hour <= 12)
+			return DS1305_HR_12 | BIN2BCD(hour);
+		hour -= 12;
+		return DS1305_HR_12 | DS1305_HR_PM | BIN2BCD(hour);
+	}
+	return BIN2BCD(hour);
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Interface to RTC framework
+ */
+
+#ifdef CONFIG_RTC_INTF_DEV
+
+/*
+ * Context: caller holds rtc->ops_lock (to protect ds1305->ctrl)
+ */
+static int ds1305_ioctl(struct device *dev, unsigned cmd, unsigned long arg)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	u8		buf[2];
+	int		status = -ENOIOCTLCMD;
+
+	buf[0] = DS1305_WRITE | DS1305_CONTROL;
+	buf[1] = ds1305->ctrl[0];
+
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		status = 0;
+		if (!(buf[1] & DS1305_AEI0))
+			goto done;
+		buf[1] &= ~DS1305_AEI0;
+		break;
+
+	case RTC_AIE_ON:
+		status = 0;
+		if (ds1305->ctrl[0] & DS1305_AEI0)
+			goto done;
+		buf[1] |= DS1305_AEI0;
+		break;
+	}
+	if (status == 0) {
+		status = spi_write_then_read(ds1305->spi, buf, sizeof buf,
+				NULL, 0);
+		if (status >= 0)
+			ds1305->ctrl[0] = buf[1];
+	}
+
+done:
+	return status;
+}
+
+#else
+#define ds1305_ioctl	NULL
+#endif
+
+/*
+ * Get/set of date and time is pretty normal.
+ */
+
+static int ds1305_get_time(struct device *dev, struct rtc_time *time)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	u8		addr = DS1305_SEC;
+	u8		buf[DS1305_RTC_LEN];
+	int		status;
+
+	/* Use write-then-read to get all the date/time registers
+	 * since dma from stack is nonportable
+	 */
+	status = spi_write_then_read(ds1305->spi, &addr, sizeof addr,
+			buf, sizeof buf);
+	if (status < 0)
+		return status;
+
+	dev_vdbg(dev, "%s: %02x %02x %02x, %02x %02x %02x %02x\n",
+		"read", buf[0], buf[1], buf[2], buf[3],
+		buf[4], buf[5], buf[6]);
+
+	/* Decode the registers */
+	time->tm_sec = BCD2BIN(buf[DS1305_SEC]);
+	time->tm_min = BCD2BIN(buf[DS1305_MIN]);
+	time->tm_hour = bcd2hour(buf[DS1305_HOUR]);
+	time->tm_wday = buf[DS1305_WDAY] - 1;
+	time->tm_mday = BCD2BIN(buf[DS1305_MDAY]);
+	time->tm_mon = BCD2BIN(buf[DS1305_MON]) - 1;
+	time->tm_year = BCD2BIN(buf[DS1305_YEAR]) + 100;
+
+	dev_vdbg(dev, "%s secs=%d, mins=%d, "
+		"hours=%d, mday=%d, mon=%d, year=%d, wday=%d\n",
+		"read", time->tm_sec, time->tm_min,
+		time->tm_hour, time->tm_mday,
+		time->tm_mon, time->tm_year, time->tm_wday);
+
+	/* Time may not be set */
+	return rtc_valid_tm(time);
+}
+
+static int ds1305_set_time(struct device *dev, struct rtc_time *time)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	u8		buf[1 + DS1305_RTC_LEN];
+	u8		*bp = buf;
+
+	dev_vdbg(dev, "%s secs=%d, mins=%d, "
+		"hours=%d, mday=%d, mon=%d, year=%d, wday=%d\n",
+		"write", time->tm_sec, time->tm_min,
+		time->tm_hour, time->tm_mday,
+		time->tm_mon, time->tm_year, time->tm_wday);
+
+	/* Write registers starting at the first time/date address. */
+	*bp++ = DS1305_WRITE | DS1305_SEC;
+
+	*bp++ = BIN2BCD(time->tm_sec);
+	*bp++ = BIN2BCD(time->tm_min);
+	*bp++ = hour2bcd(ds1305->hr12, time->tm_hour);
+	*bp++ = (time->tm_wday < 7) ? (time->tm_wday + 1) : 1;
+	*bp++ = BIN2BCD(time->tm_mday);
+	*bp++ = BIN2BCD(time->tm_mon + 1);
+	*bp++ = BIN2BCD(time->tm_year - 100);
+
+	dev_dbg(dev, "%s: %02x %02x %02x, %02x %02x %02x %02x\n",
+		"write", buf[1], buf[2], buf[3],
+		buf[4], buf[5], buf[6], buf[7]);
+
+	/* use write-then-read since dma from stack is nonportable */
+	return spi_write_then_read(ds1305->spi, buf, sizeof buf,
+			NULL, 0);
+}
+
+/*
+ * Get/set of alarm is a bit funky:
+ *
+ * - First there's the inherent raciness of getting the (partitioned)
+ *   status of an alarm that could trigger while we're reading parts
+ *   of that status.
+ *
+ * - Second there's its limited range (we could increase it a bit by
+ *   relying on WDAY), which means it will easily roll over.
+ *
+ * - Third there's the choice of two alarms and alarm signals.
+ *   Here we use ALM0 and expect that nINT0 (open drain) is used;
+ *   that's the only real option for DS1306 runtime alarms, and is
+ *   natural on DS1305.
+ *
+ * - Fourth, there's also ALM1, and a second interrupt signal:
+ *     + On DS1305 ALM1 uses nINT1 (when INTCN=1) else nINT0;
+ *     + On DS1306 ALM1 only uses INT1 (an active high pulse)
+ *       and it won't work when VCC1 is active.
+ *
+ *   So to be most general, we should probably set both alarms to the
+ *   same value, letting ALM1 be the wakeup event source on DS1306
+ *   and handling several wiring options on DS1305.
+ *
+ * - Fifth, we support the polled mode (as well as possible; why not?)
+ *   even when no interrupt line is wired to an IRQ.
+ */
+
+/*
+ * Context: caller holds rtc->ops_lock (to protect ds1305->ctrl)
+ */
+static int ds1305_get_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	struct spi_device *spi = ds1305->spi;
+	u8		addr;
+	int		status;
+	u8		buf[DS1305_ALM_LEN];
+
+	/* Refresh control register cache BEFORE reading ALM0 registers,
+	 * since reading alarm registers acks any pending IRQ.  That
+	 * makes returning "pending" status a bit of a lie, but that bit
+	 * of EFI status is at best fragile anyway (given IRQ handlers).
+	 */
+	addr = DS1305_CONTROL;
+	status = spi_write_then_read(spi, &addr, sizeof addr,
+			ds1305->ctrl, sizeof ds1305->ctrl);
+	if (status < 0)
+		return status;
+
+	alm->enabled = !!(ds1305->ctrl[0] & DS1305_AEI0);
+	alm->pending = !!(ds1305->ctrl[1] & DS1305_AEI0);
+
+	/* get and check ALM0 registers */
+	addr = DS1305_ALM0(DS1305_SEC);
+	status = spi_write_then_read(spi, &addr, sizeof addr,
+			buf, sizeof buf);
+	if (status < 0)
+		return status;
+
+	dev_vdbg(dev, "%s: %02x %02x %02x %02x\n",
+		"alm0 read", buf[DS1305_SEC], buf[DS1305_MIN],
+		buf[DS1305_HOUR], buf[DS1305_WDAY]);
+
+	if ((DS1305_ALM_DISABLE & buf[DS1305_SEC])
+			|| (DS1305_ALM_DISABLE & buf[DS1305_MIN])
+			|| (DS1305_ALM_DISABLE & buf[DS1305_HOUR]))
+		return -EIO;
+
+	/* Stuff these values into alm->time and let RTC framework code
+	 * fill in the rest ... and also handle rollover to tomorrow when
+	 * that's needed.
+	 */
+	alm->time.tm_sec = BCD2BIN(buf[DS1305_SEC]);
+	alm->time.tm_min = BCD2BIN(buf[DS1305_MIN]);
+	alm->time.tm_hour = bcd2hour(buf[DS1305_HOUR]);
+	alm->time.tm_mday = -1;
+	alm->time.tm_mon = -1;
+	alm->time.tm_year = -1;
+	/* next three fields are unused by Linux */
+	alm->time.tm_wday = -1;
+	alm->time.tm_mday = -1;
+	alm->time.tm_isdst = -1;
+
+	return 0;
+}
+
+/*
+ * Context: caller holds rtc->ops_lock (to protect ds1305->ctrl)
+ */
+static int ds1305_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	struct spi_device *spi = ds1305->spi;
+	unsigned long	now, later;
+	struct rtc_time	tm;
+	int		status;
+	u8		buf[1 + DS1305_ALM_LEN];
+
+	/* convert desired alarm to time_t */
+	status = rtc_tm_to_time(&alm->time, &later);
+	if (status < 0)
+		return status;
+
+	/* Read current time as time_t */
+	status = ds1305_get_time(dev, &tm);
+	if (status < 0)
+		return status;
+	status = rtc_tm_to_time(&tm, &now);
+	if (status < 0)
+		return status;
+
+	/* make sure alarm fires within the next 24 hours */
+	if (later <= now)
+		return -EINVAL;
+	if ((later - now) > 24 * 60 * 60)
+		return -EDOM;
+
+	/* disable alarm if needed */
+	if (ds1305->ctrl[0] & DS1305_AEI0) {
+		ds1305->ctrl[0] &= ~DS1305_AEI0;
+
+		buf[0] = DS1305_WRITE | DS1305_CONTROL;
+		buf[1] = ds1305->ctrl[0];
+		status = spi_write_then_read(ds1305->spi, buf, 2, NULL, 0);
+		if (status < 0)
+			return status;
+	}
+
+	/* write alarm */
+	buf[0] = DS1305_WRITE | DS1305_ALM0(DS1305_SEC);
+	buf[1 + DS1305_SEC] = BIN2BCD(alm->time.tm_sec);
+	buf[1 + DS1305_MIN] = BIN2BCD(alm->time.tm_min);
+	buf[1 + DS1305_HOUR] = hour2bcd(ds1305->hr12, alm->time.tm_hour);
+	buf[1 + DS1305_WDAY] = DS1305_ALM_DISABLE;
+
+	dev_dbg(dev, "%s: %02x %02x %02x %02x\n",
+		"alm0 write", buf[1 + DS1305_SEC], buf[1 + DS1305_MIN],
+		buf[1 + DS1305_HOUR], buf[1 + DS1305_WDAY]);
+
+	status = spi_write_then_read(spi, buf, sizeof buf, NULL, 0);
+	if (status < 0)
+		return status;
+
+	/* enable alarm if requested */
+	if (alm->enabled) {
+		ds1305->ctrl[0] |= DS1305_AEI0;
+
+		buf[0] = DS1305_WRITE | DS1305_CONTROL;
+		buf[1] = ds1305->ctrl[0];
+		status = spi_write_then_read(ds1305->spi, buf, 2, NULL, 0);
+	}
+
+	return status;
+}
+
+#ifdef CONFIG_PROC_FS
+
+static int ds1305_proc(struct device *dev, struct seq_file *seq)
+{
+	struct ds1305	*ds1305 = dev_get_drvdata(dev);
+	char		*diodes = "no";
+	char		*resistors = "";
+
+	/* ctrl[2] is treated as read-only; no locking needed */
+	if ((ds1305->ctrl[2] & 0xf0) == DS1305_TRICKLE_MAGIC) {
+		switch (ds1305->ctrl[2] & 0x0c) {
+		case DS1305_TRICKLE_DS2:
+			diodes = "2 diodes, ";
+			break;
+		case DS1305_TRICKLE_DS1:
+			diodes = "1 diode, ";
+			break;
+		default:
+			goto done;
+		}
+		switch (ds1305->ctrl[2] & 0x03) {
+		case DS1305_TRICKLE_2K:
+			resistors = "2k Ohm";
+			break;
+		case DS1305_TRICKLE_4K:
+			resistors = "4k Ohm";
+			break;
+		case DS1305_TRICKLE_8K:
+			resistors = "8k Ohm";
+			break;
+		default:
+			diodes = "no";
+			break;
+		}
+	}
+
+done:
+	return seq_printf(seq,
+			"trickle_charge\t: %s%s\n",
+			diodes, resistors);
+}
+
+#else
+#define ds1305_proc	NULL
+#endif
+
+static const struct rtc_class_ops ds1305_ops = {
+	.ioctl		= ds1305_ioctl,
+	.read_time	= ds1305_get_time,
+	.set_time	= ds1305_set_time,
+	.read_alarm	= ds1305_get_alarm,
+	.set_alarm	= ds1305_set_alarm,
+	.proc		= ds1305_proc,
+};
+
+static void ds1305_work(struct work_struct *work)
+{
+	struct ds1305	*ds1305 = container_of(work, struct ds1305, work);
+	struct mutex	*lock = &ds1305->rtc->ops_lock;
+	struct spi_device *spi = ds1305->spi;
+	u8		buf[3];
+	int		status;
+
+	/* lock to protect ds1305->ctrl */
+	mutex_lock(lock);
+
+	/* Disable the IRQ, and clear its status ... for now, we "know"
+	 * that if more than one alarm is active, they're in sync.
+	 * Note that reading ALM data registers also clears IRQ status.
+	 */
+	ds1305->ctrl[0] &= ~(DS1305_AEI1 | DS1305_AEI0);
+	ds1305->ctrl[1] = 0;
+
+	buf[0] = DS1305_WRITE | DS1305_CONTROL;
+	buf[1] = ds1305->ctrl[0];
+	buf[2] = 0;
+
+	status = spi_write_then_read(spi, buf, sizeof buf,
+			NULL, 0);
+	if (status < 0)
+		dev_dbg(&spi->dev, "clear irq --> %d\n", status);
+
+	mutex_unlock(lock);
+
+	if (!test_bit(FLAG_EXITING, &ds1305->flags))
+		enable_irq(spi->irq);
+
+	/* rtc_update_irq() requires an IRQ-disabled context */
+	local_irq_disable();
+	rtc_update_irq(ds1305->rtc, 1, RTC_AF | RTC_IRQF);
+	local_irq_enable();
+}
+
+/*
+ * This "real" IRQ handler hands off to a workqueue mostly to allow
+ * mutex locking for ds1305->ctrl ... unlike I2C, we could issue async
+ * I/O requests in IRQ context (to clear the IRQ status).
+ */
+static irqreturn_t ds1305_irq(int irq, void *p)
+{
+	struct ds1305		*ds1305 = p;
+
+	disable_irq(irq);
+	schedule_work(&ds1305->work);
+	return IRQ_HANDLED;
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Interface for NVRAM
+ */
+
+static void msg_init(struct spi_message *m, struct spi_transfer *x,
+		u8 *addr, size_t count, char *tx, char *rx)
+{
+	spi_message_init(m);
+	memset(x, 0, 2 * sizeof(*x));
+
+	x->tx_buf = addr;
+	x->len = 1;
+	spi_message_add_tail(x, m);
+
+	x++;
+
+	x->tx_buf = tx;
+	x->rx_buf = rx;
+	x->len = count;
+	spi_message_add_tail(x, m);
+}
+
+static ssize_t
+ds1305_nvram_read(struct kobject *kobj, struct bin_attribute *attr,
+		char *buf, loff_t off, size_t count)
+{
+	struct spi_device	*spi;
+	u8			addr;
+	struct spi_message	m;
+	struct spi_transfer	x[2];
+	int			status;
+
+	spi = container_of(kobj, struct spi_device, dev.kobj);
+
+	if (unlikely(off >= DS1305_NVRAM_LEN))
+		return 0;
+	if (count >= DS1305_NVRAM_LEN)
+		count = DS1305_NVRAM_LEN;
+	if ((off + count) > DS1305_NVRAM_LEN)
+		count = DS1305_NVRAM_LEN - off;
+	if (unlikely(!count))
+		return count;
+
+	addr = DS1305_NVRAM + off;
+	msg_init(&m, x, &addr, count, NULL, buf);
+
+	status = spi_sync(spi, &m);
+	if (status < 0)
+		dev_err(&spi->dev, "nvram %s error %d\n", "read", status);
+	return (status < 0) ? status : count;
+}
+
+static ssize_t
+ds1305_nvram_write(struct kobject *kobj, struct bin_attribute *attr,
+		char *buf, loff_t off, size_t count)
+{
+	struct spi_device	*spi;
+	u8			addr;
+	struct spi_message	m;
+	struct spi_transfer	x[2];
+	int			status;
+
+	spi = container_of(kobj, struct spi_device, dev.kobj);
+
+	if (unlikely(off >= DS1305_NVRAM_LEN))
+		return -EFBIG;
+	if (count >= DS1305_NVRAM_LEN)
+		count = DS1305_NVRAM_LEN;
+	if ((off + count) > DS1305_NVRAM_LEN)
+		count = DS1305_NVRAM_LEN - off;
+	if (unlikely(!count))
+		return count;
+
+	addr = (DS1305_WRITE | DS1305_NVRAM) + off;
+	msg_init(&m, x, &addr, count, buf, NULL);
+
+	status = spi_sync(spi, &m);
+	if (status < 0)
+		dev_err(&spi->dev, "nvram %s error %d\n", "write", status);
+	return (status < 0) ? status : count;
+}
+
+static struct bin_attribute nvram = {
+	.attr.name	= "nvram",
+	.attr.mode	= S_IRUGO | S_IWUSR,
+	.attr.owner	= THIS_MODULE,
+	.read		= ds1305_nvram_read,
+	.write		= ds1305_nvram_write,
+	.size		= DS1305_NVRAM_LEN,
+};
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Interface to SPI stack
+ */
+
+static int __devinit ds1305_probe(struct spi_device *spi)
+{
+	struct ds1305			*ds1305;
+	struct rtc_device		*rtc;
+	int				status;
+	u8				addr, value;
+	struct ds1305_platform_data	*pdata = spi->dev.platform_data;
+	bool				write_ctrl = false;
+
+	/* Sanity check board setup data.  This may be hooked up
+	 * in 3wire mode, but we don't care.  Note that unless
+	 * there's an inverter in place, this needs SPI_CS_HIGH!
+	 */
+	if ((spi->bits_per_word && spi->bits_per_word != 8)
+			|| (spi->max_speed_hz > 2000000)
+			|| !(spi->mode & SPI_CPHA))
+		return -EINVAL;
+
+	/* set up driver data */
+	ds1305 = kzalloc(sizeof *ds1305, GFP_KERNEL);
+	if (!ds1305)
+		return -ENOMEM;
+	ds1305->spi = spi;
+	spi_set_drvdata(spi, ds1305);
+
+	/* read and cache control registers */
+	addr = DS1305_CONTROL;
+	status = spi_write_then_read(spi, &addr, sizeof addr,
+			ds1305->ctrl, sizeof ds1305->ctrl);
+	if (status < 0) {
+		dev_dbg(&spi->dev, "can't %s, %d\n",
+				"read", status);
+		goto fail0;
+	}
+
+	dev_dbg(&spi->dev, "ctrl %s: %02x %02x %02x\n",
+			"read", ds1305->ctrl[0],
+			ds1305->ctrl[1], ds1305->ctrl[2]);
+
+	/* Sanity check register values ... partially compensating for the
+	 * fact that SPI has no device handshake.  A pullup on MISO would
+	 * make these tests fail; but not all systems will have one.  If
+	 * some register is neither 0x00 nor 0xff, a chip is likely there.
+	 */
+	if ((ds1305->ctrl[0] & 0x38) != 0 || (ds1305->ctrl[1] & 0xfc) != 0) {
+		dev_dbg(&spi->dev, "RTC chip is not present\n");
+		status = -ENODEV;
+		goto fail0;
+	}
+	if (ds1305->ctrl[2] == 0)
+		dev_dbg(&spi->dev, "chip may not be present\n");
+
+	/* enable writes if needed ... if we were paranoid it would
+	 * make sense to enable them only when absolutely necessary.
+	 */
+	if (ds1305->ctrl[0] & DS1305_WP) {
+		u8		buf[2];
+
+		ds1305->ctrl[0] &= ~DS1305_WP;
+
+		buf[0] = DS1305_WRITE | DS1305_CONTROL;
+		buf[1] = ds1305->ctrl[0];
+		status = spi_write_then_read(spi, buf, sizeof buf, NULL, 0);
+
+		dev_dbg(&spi->dev, "clear WP --> %d\n", status);
+		if (status < 0)
+			goto fail0;
+	}
+
+	/* on DS1305, maybe start oscillator; like most low power
+	 * oscillators, it may take a second to stabilize
+	 */
+	if (ds1305->ctrl[0] & DS1305_nEOSC) {
+		ds1305->ctrl[0] &= ~DS1305_nEOSC;
+		write_ctrl = true;
+		dev_warn(&spi->dev, "SET TIME!\n");
+	}
+
+	/* ack any pending IRQs */
+	if (ds1305->ctrl[1]) {
+		ds1305->ctrl[1] = 0;
+		write_ctrl = true;
+	}
+
+	/* this may need one-time (re)init */
+	if (pdata) {
+		/* maybe enable trickle charge */
+		if (((ds1305->ctrl[2] & 0xf0) != DS1305_TRICKLE_MAGIC)) {
+			ds1305->ctrl[2] = DS1305_TRICKLE_MAGIC
+						| pdata->trickle;
+			write_ctrl = true;
+		}
+
+		/* on DS1306, configure 1 Hz signal */
+		if (pdata->is_ds1306) {
+			if (pdata->en_1hz) {
+				if (!(ds1305->ctrl[0] & DS1306_1HZ)) {
+					ds1305->ctrl[0] |= DS1306_1HZ;
+					write_ctrl = true;
+				}
+			} else {
+				if (ds1305->ctrl[0] & DS1306_1HZ) {
+					ds1305->ctrl[0] &= ~DS1306_1HZ;
+					write_ctrl = true;
+				}
+			}
+		}
+	}
+
+	if (write_ctrl) {
+		u8		buf[4];
+
+		buf[0] = DS1305_WRITE | DS1305_CONTROL;
+		buf[1] = ds1305->ctrl[0];
+		buf[2] = ds1305->ctrl[1];
+		buf[3] = ds1305->ctrl[2];
+		status = spi_write_then_read(spi, buf, sizeof buf, NULL, 0);
+		if (status < 0) {
+			dev_dbg(&spi->dev, "can't %s, %d\n",
+					"write", status);
+			goto fail0;
+		}
+
+		dev_dbg(&spi->dev, "ctrl %s: %02x %02x %02x\n",
+				"write", ds1305->ctrl[0],
+				ds1305->ctrl[1], ds1305->ctrl[2]);
+	}
+
+	/* see if non-Linux software set up AM/PM mode */
+	addr = DS1305_HOUR;
+	status = spi_write_then_read(spi, &addr, sizeof addr,
+				&value, sizeof value);
+	if (status < 0) {
+		dev_dbg(&spi->dev, "read HOUR --> %d\n", status);
+		goto fail0;
+	}
+
+	ds1305->hr12 = (DS1305_HR_12 & value) != 0;
+	if (ds1305->hr12)
+		dev_dbg(&spi->dev, "AM/PM\n");
+
+	/* register RTC ... from here on, ds1305->ctrl needs locking */
+	rtc = rtc_device_register("ds1305", &spi->dev,
+			&ds1305_ops, THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		status = PTR_ERR(rtc);
+		dev_dbg(&spi->dev, "register rtc --> %d\n", status);
+		goto fail0;
+	}
+	ds1305->rtc = rtc;
+
+	/* Maybe set up alarm IRQ; be ready to handle it triggering right
+	 * away.  NOTE that we don't share this.  The signal is active low,
+	 * and we can't ack it before a SPI message delay.  We temporarily
+	 * disable the IRQ until it's acked, which lets us work with more
+	 * IRQ trigger modes (not all IRQ controllers can do falling edge).
+	 */
+	if (spi->irq) {
+		INIT_WORK(&ds1305->work, ds1305_work);
+		status = request_irq(spi->irq, ds1305_irq,
+				0, dev_name(&rtc->dev), ds1305);
+		if (status < 0) {
+			dev_dbg(&spi->dev, "request_irq %d --> %d\n",
+					spi->irq, status);
+			goto fail1;
+		}
+	}
+
+	/* export NVRAM */
+	status = sysfs_create_bin_file(&spi->dev.kobj, &nvram);
+	if (status < 0) {
+		dev_dbg(&spi->dev, "register nvram --> %d\n", status);
+		goto fail2;
+	}
+
+	return 0;
+
+fail2:
+	free_irq(spi->irq, ds1305);
+fail1:
+	rtc_device_unregister(rtc);
+fail0:
+	kfree(ds1305);
+	return status;
+}
+
+static int __devexit ds1305_remove(struct spi_device *spi)
+{
+	struct ds1305	*ds1305 = spi_get_drvdata(spi);
+
+	sysfs_remove_bin_file(&spi->dev.kobj, &nvram);
+
+	/* carefully shut down irq and workqueue, if present */
+	if (spi->irq) {
+		set_bit(FLAG_EXITING, &ds1305->flags);
+		free_irq(spi->irq, ds1305);
+		flush_scheduled_work();
+	}
+
+	rtc_device_unregister(ds1305->rtc);
+	spi_set_drvdata(spi, NULL);
+	kfree(ds1305);
+	return 0;
+}
+
+static struct spi_driver ds1305_driver = {
+	.driver.name	= "rtc-ds1305",
+	.driver.owner	= THIS_MODULE,
+	.probe		= ds1305_probe,
+	.remove		= __devexit_p(ds1305_remove),
+	/* REVISIT add suspend/resume */
+};
+
+static int __init ds1305_init(void)
+{
+	return spi_register_driver(&ds1305_driver);
+}
+module_init(ds1305_init);
+
+static void __exit ds1305_exit(void)
+{
+	spi_unregister_driver(&ds1305_driver);
+}
+module_exit(ds1305_exit);
+
+MODULE_DESCRIPTION("RTC driver for DS1305 and DS1306 chips");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index 0a19c06019b..24bc1689fc7 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -13,21 +13,21 @@
  *
  */
 
-#include <linux/module.h>
+#include <linux/bcd.h>
+#include <linux/i2c.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/rtc.h>
 #include <linux/slab.h>
 #include <linux/smp_lock.h>
 #include <linux/string.h>
-#include <linux/i2c.h>
-#include <linux/rtc.h>
-#include <linux/bcd.h>
 #ifdef CONFIG_RTC_DRV_M41T80_WDT
-#include <linux/miscdevice.h>
-#include <linux/watchdog.h>
-#include <linux/reboot.h>
 #include <linux/fs.h>
 #include <linux/ioctl.h>
+#include <linux/miscdevice.h>
+#include <linux/reboot.h>
+#include <linux/watchdog.h>
 #endif
 
 #define M41T80_REG_SSEC	0
@@ -631,14 +631,12 @@ static int wdt_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
 			return -EFAULT;
 
 		if (rv & WDIOS_DISABLECARD) {
-			printk(KERN_INFO
-			       "rtc-m41t80: disable watchdog\n");
+			pr_info("rtc-m41t80: disable watchdog\n");
 			wdt_disable();
 		}
 
 		if (rv & WDIOS_ENABLECARD) {
-			printk(KERN_INFO
-			       "rtc-m41t80: enable watchdog\n");
+			pr_info("rtc-m41t80: enable watchdog\n");
 			wdt_ping();
 		}
 
diff --git a/drivers/rtc/rtc-m41t94.c b/drivers/rtc/rtc-m41t94.c
new file mode 100644
index 00000000000..9b19499c829
--- /dev/null
+++ b/drivers/rtc/rtc-m41t94.c
@@ -0,0 +1,173 @@
+/*
+ * Driver for ST M41T94 SPI RTC
+ *
+ * Copyright (C) 2008 Kim B. Heino
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/spi/spi.h>
+#include <linux/bcd.h>
+
+#define M41T94_REG_SECONDS	0x01
+#define M41T94_REG_MINUTES	0x02
+#define M41T94_REG_HOURS	0x03
+#define M41T94_REG_WDAY		0x04
+#define M41T94_REG_DAY		0x05
+#define M41T94_REG_MONTH	0x06
+#define M41T94_REG_YEAR		0x07
+#define M41T94_REG_HT		0x0c
+
+#define M41T94_BIT_HALT		0x40
+#define M41T94_BIT_STOP		0x80
+#define M41T94_BIT_CB		0x40
+#define M41T94_BIT_CEB		0x80
+
+static int m41t94_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	u8 buf[8]; /* write cmd + 7 registers */
+
+	dev_dbg(dev, "%s secs=%d, mins=%d, "
+		"hours=%d, mday=%d, mon=%d, year=%d, wday=%d\n",
+		"write", tm->tm_sec, tm->tm_min,
+		tm->tm_hour, tm->tm_mday,
+		tm->tm_mon, tm->tm_year, tm->tm_wday);
+
+	buf[0] = 0x80 | M41T94_REG_SECONDS; /* write time + date */
+	buf[M41T94_REG_SECONDS] = BIN2BCD(tm->tm_sec);
+	buf[M41T94_REG_MINUTES] = BIN2BCD(tm->tm_min);
+	buf[M41T94_REG_HOURS]   = BIN2BCD(tm->tm_hour);
+	buf[M41T94_REG_WDAY]    = BIN2BCD(tm->tm_wday + 1);
+	buf[M41T94_REG_DAY]     = BIN2BCD(tm->tm_mday);
+	buf[M41T94_REG_MONTH]   = BIN2BCD(tm->tm_mon + 1);
+
+	buf[M41T94_REG_HOURS] |= M41T94_BIT_CEB;
+	if (tm->tm_year >= 100)
+		buf[M41T94_REG_HOURS] |= M41T94_BIT_CB;
+	buf[M41T94_REG_YEAR] = BIN2BCD(tm->tm_year % 100);
+
+	return spi_write(spi, buf, 8);
+}
+
+static int m41t94_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	u8 buf[2];
+	int ret, hour;
+
+	/* clear halt update bit */
+	ret = spi_w8r8(spi, M41T94_REG_HT);
+	if (ret < 0)
+		return ret;
+	if (ret & M41T94_BIT_HALT) {
+		buf[0] = 0x80 | M41T94_REG_HT;
+		buf[1] = ret & ~M41T94_BIT_HALT;
+		spi_write(spi, buf, 2);
+	}
+
+	/* clear stop bit */
+	ret = spi_w8r8(spi, M41T94_REG_SECONDS);
+	if (ret < 0)
+		return ret;
+	if (ret & M41T94_BIT_STOP) {
+		buf[0] = 0x80 | M41T94_REG_SECONDS;
+		buf[1] = ret & ~M41T94_BIT_STOP;
+		spi_write(spi, buf, 2);
+	}
+
+	tm->tm_sec  = BCD2BIN(spi_w8r8(spi, M41T94_REG_SECONDS));
+	tm->tm_min  = BCD2BIN(spi_w8r8(spi, M41T94_REG_MINUTES));
+	hour = spi_w8r8(spi, M41T94_REG_HOURS);
+	tm->tm_hour = BCD2BIN(hour & 0x3f);
+	tm->tm_wday = BCD2BIN(spi_w8r8(spi, M41T94_REG_WDAY)) - 1;
+	tm->tm_mday = BCD2BIN(spi_w8r8(spi, M41T94_REG_DAY));
+	tm->tm_mon  = BCD2BIN(spi_w8r8(spi, M41T94_REG_MONTH)) - 1;
+	tm->tm_year = BCD2BIN(spi_w8r8(spi, M41T94_REG_YEAR));
+	if ((hour & M41T94_BIT_CB) || !(hour & M41T94_BIT_CEB))
+		tm->tm_year += 100;
+
+	dev_dbg(dev, "%s secs=%d, mins=%d, "
+		"hours=%d, mday=%d, mon=%d, year=%d, wday=%d\n",
+		"read", tm->tm_sec, tm->tm_min,
+		tm->tm_hour, tm->tm_mday,
+		tm->tm_mon, tm->tm_year, tm->tm_wday);
+
+	/* initial clock setting can be undefined */
+	return rtc_valid_tm(tm);
+}
+
+static const struct rtc_class_ops m41t94_rtc_ops = {
+	.read_time	= m41t94_read_time,
+	.set_time	= m41t94_set_time,
+};
+
+static struct spi_driver m41t94_driver;
+
+static int __devinit m41t94_probe(struct spi_device *spi)
+{
+	struct rtc_device *rtc;
+	int res;
+
+	spi->bits_per_word = 8;
+	spi_setup(spi);
+
+	res = spi_w8r8(spi, M41T94_REG_SECONDS);
+	if (res < 0) {
+		dev_err(&spi->dev, "not found.\n");
+		return res;
+	}
+
+	rtc = rtc_device_register(m41t94_driver.driver.name,
+		&spi->dev, &m41t94_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+
+	dev_set_drvdata(&spi->dev, rtc);
+
+	return 0;
+}
+
+static int __devexit m41t94_remove(struct spi_device *spi)
+{
+	struct rtc_device *rtc = platform_get_drvdata(spi);
+
+	if (rtc)
+		rtc_device_unregister(rtc);
+
+	return 0;
+}
+
+static struct spi_driver m41t94_driver = {
+	.driver = {
+		.name	= "rtc-m41t94",
+		.bus	= &spi_bus_type,
+		.owner	= THIS_MODULE,
+	},
+	.probe	= m41t94_probe,
+	.remove = __devexit_p(m41t94_remove),
+};
+
+static __init int m41t94_init(void)
+{
+	return spi_register_driver(&m41t94_driver);
+}
+
+module_init(m41t94_init);
+
+static __exit void m41t94_exit(void)
+{
+	spi_unregister_driver(&m41t94_driver);
+}
+
+module_exit(m41t94_exit);
+
+MODULE_AUTHOR("Kim B. Heino <Kim.Heino@bluegiga.com>");
+MODULE_DESCRIPTION("Driver for ST M41T94 SPI RTC");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-omap.c b/drivers/rtc/rtc-omap.c
index eb23d8423f4..8876605d4d4 100644
--- a/drivers/rtc/rtc-omap.c
+++ b/drivers/rtc/rtc-omap.c
@@ -92,18 +92,6 @@
 #define rtc_write(val, addr)	omap_writeb(val, OMAP_RTC_BASE + (addr))
 
 
-/* platform_bus isn't hotpluggable, so for static linkage it'd be safe
- * to get rid of probe() and remove() code ... too bad the driver struct
- * remembers probe(), that's about 25% of the runtime footprint!!
- */
-#ifndef	MODULE
-#undef	__devexit
-#undef	__devexit_p
-#define	__devexit	__exit
-#define	__devexit_p	__exit_p
-#endif
-
-
 /* we rely on the rtc framework to handle locking (rtc->ops_lock),
  * so the only other requirement is that register accesses which
  * require BUSY to be clear are made with IRQs locally disabled
@@ -324,7 +312,7 @@ static struct rtc_class_ops omap_rtc_ops = {
 static int omap_rtc_alarm;
 static int omap_rtc_timer;
 
-static int __devinit omap_rtc_probe(struct platform_device *pdev)
+static int __init omap_rtc_probe(struct platform_device *pdev)
 {
 	struct resource		*res, *mem;
 	struct rtc_device	*rtc;
@@ -440,7 +428,7 @@ fail:
 	return -EIO;
 }
 
-static int __devexit omap_rtc_remove(struct platform_device *pdev)
+static int __exit omap_rtc_remove(struct platform_device *pdev)
 {
 	struct rtc_device	*rtc = platform_get_drvdata(pdev);;
 
@@ -498,8 +486,7 @@ static void omap_rtc_shutdown(struct platform_device *pdev)
 
 MODULE_ALIAS("platform:omap_rtc");
 static struct platform_driver omap_rtc_driver = {
-	.probe		= omap_rtc_probe,
-	.remove		= __devexit_p(omap_rtc_remove),
+	.remove		= __exit_p(omap_rtc_remove),
 	.suspend	= omap_rtc_suspend,
 	.resume		= omap_rtc_resume,
 	.shutdown	= omap_rtc_shutdown,
@@ -511,7 +498,7 @@ static struct platform_driver omap_rtc_driver = {
 
 static int __init rtc_init(void)
 {
-	return platform_driver_register(&omap_rtc_driver);
+	return platform_driver_probe(&omap_rtc_driver, omap_rtc_probe);
 }
 module_init(rtc_init);
 
diff --git a/drivers/rtc/rtc-pcf8583.c b/drivers/rtc/rtc-pcf8583.c
index 3d09d8f0b1f..d388c662bf4 100644
--- a/drivers/rtc/rtc-pcf8583.c
+++ b/drivers/rtc/rtc-pcf8583.c
@@ -2,6 +2,7 @@
  *  drivers/rtc/rtc-pcf8583.c
  *
  *  Copyright (C) 2000 Russell King
+ *  Copyright (C) 2008 Wolfram Sang & Juergen Beisert, Pengutronix
  *
  * 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
@@ -14,7 +15,6 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
-#include <linux/string.h>
 #include <linux/rtc.h>
 #include <linux/init.h>
 #include <linux/errno.h>
@@ -27,7 +27,6 @@ struct rtc_mem {
 };
 
 struct pcf8583 {
-	struct i2c_client client;
 	struct rtc_device *rtc;
 	unsigned char ctrl;
 };
@@ -40,10 +39,6 @@ struct pcf8583 {
 #define CTRL_ALARM	0x02
 #define CTRL_TIMER	0x01
 
-static const unsigned short normal_i2c[] = { 0x50, I2C_CLIENT_END };
-
-/* Module parameters */
-I2C_CLIENT_INSMOD;
 
 static struct i2c_driver pcf8583_driver;
 
@@ -269,106 +264,60 @@ static const struct rtc_class_ops pcf8583_rtc_ops = {
 	.set_time	= pcf8583_rtc_set_time,
 };
 
-static int pcf8583_probe(struct i2c_adapter *adap, int addr, int kind);
-
-static int pcf8583_attach(struct i2c_adapter *adap)
-{
-	return i2c_probe(adap, &addr_data, pcf8583_probe);
-}
-
-static int pcf8583_detach(struct i2c_client *client)
-{
-	int err;
-	struct pcf8583 *pcf = i2c_get_clientdata(client);
-	struct rtc_device *rtc = pcf->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
-	if ((err = i2c_detach_client(client)))
-		return err;
-
-	kfree(pcf);
-	return 0;
-}
-
-static struct i2c_driver pcf8583_driver = {
-	.driver = {
-		.name	= "pcf8583",
-	},
-	.id		= I2C_DRIVERID_PCF8583,
-	.attach_adapter	= pcf8583_attach,
-	.detach_client	= pcf8583_detach,
-};
-
-static int pcf8583_probe(struct i2c_adapter *adap, int addr, int kind)
+static int pcf8583_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
 {
-	struct pcf8583 *pcf;
-	struct i2c_client *client;
-	struct rtc_device *rtc;
-	unsigned char buf[1], ad[1] = { 0 };
+	struct pcf8583 *pcf8583;
 	int err;
-	struct i2c_msg msgs[2] = {
-		{
-			.addr = addr,
-			.flags = 0,
-			.len = 1,
-			.buf = ad,
-		}, {
-			.addr = addr,
-			.flags = I2C_M_RD,
-			.len = 1,
-			.buf = buf,
-		}
-	};
 
-	if (!i2c_check_functionality(adap, I2C_FUNC_I2C))
-		return 0;
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
+		return -ENODEV;
 
-	pcf = kzalloc(sizeof(*pcf), GFP_KERNEL);
-	if (!pcf)
+	pcf8583 = kzalloc(sizeof(struct pcf8583), GFP_KERNEL);
+	if (!pcf8583)
 		return -ENOMEM;
 
-	client = &pcf->client;
+	pcf8583->rtc = rtc_device_register(pcf8583_driver.driver.name,
+			&client->dev, &pcf8583_rtc_ops, THIS_MODULE);
 
-	client->addr		= addr;
-	client->adapter	= adap;
-	client->driver	= &pcf8583_driver;
-
-	strlcpy(client->name, pcf8583_driver.driver.name, I2C_NAME_SIZE);
-
-	if (i2c_transfer(client->adapter, msgs, 2) != 2) {
-		err = -EIO;
+	if (IS_ERR(pcf8583->rtc)) {
+		err = PTR_ERR(pcf8583->rtc);
 		goto exit_kfree;
 	}
 
-	err = i2c_attach_client(client);
-
-	if (err)
-		goto exit_kfree;
-
-	rtc = rtc_device_register(pcf8583_driver.driver.name, &client->dev,
-				  &pcf8583_rtc_ops, THIS_MODULE);
+	i2c_set_clientdata(client, pcf8583);
+	return 0;
 
-	if (IS_ERR(rtc)) {
-		err = PTR_ERR(rtc);
-		goto exit_detach;
-	}
+exit_kfree:
+	kfree(pcf8583);
+	return err;
+}
 
-	pcf->rtc = rtc;
-	i2c_set_clientdata(client, pcf);
-	set_ctrl(client, buf[0]);
+static int __devexit pcf8583_remove(struct i2c_client *client)
+{
+	struct pcf8583 *pcf8583 = i2c_get_clientdata(client);
 
+	if (pcf8583->rtc)
+		rtc_device_unregister(pcf8583->rtc);
+	kfree(pcf8583);
 	return 0;
+}
 
-exit_detach:
-	i2c_detach_client(client);
-
-exit_kfree:
-	kfree(pcf);
+static const struct i2c_device_id pcf8583_id[] = {
+	{ "pcf8583", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, pcf8583_id);
 
-	return err;
-}
+static struct i2c_driver pcf8583_driver = {
+	.driver = {
+		.name	= "pcf8583",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= pcf8583_probe,
+	.remove		= __devexit_p(pcf8583_remove),
+	.id_table	= pcf8583_id,
+};
 
 static __init int pcf8583_init(void)
 {
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index fed86e507fd..54b1ebb0150 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -36,10 +36,8 @@ static struct resource *s3c_rtc_mem;
 static void __iomem *s3c_rtc_base;
 static int s3c_rtc_alarmno = NO_IRQ;
 static int s3c_rtc_tickno  = NO_IRQ;
-static int s3c_rtc_freq    = 1;
 
 static DEFINE_SPINLOCK(s3c_rtc_pie_lock);
-static unsigned int tick_count;
 
 /* IRQ Handlers */
 
@@ -55,7 +53,7 @@ static irqreturn_t s3c_rtc_tickirq(int irq, void *id)
 {
 	struct rtc_device *rdev = id;
 
-	rtc_update_irq(rdev, tick_count++, RTC_PF | RTC_IRQF);
+	rtc_update_irq(rdev, 1, RTC_PF | RTC_IRQF);
 	return IRQ_HANDLED;
 }
 
@@ -74,35 +72,37 @@ static void s3c_rtc_setaie(int to)
 	writeb(tmp, s3c_rtc_base + S3C2410_RTCALM);
 }
 
-static void s3c_rtc_setpie(int to)
+static int s3c_rtc_setpie(struct device *dev, int enabled)
 {
 	unsigned int tmp;
 
-	pr_debug("%s: pie=%d\n", __func__, to);
+	pr_debug("%s: pie=%d\n", __func__, enabled);
 
 	spin_lock_irq(&s3c_rtc_pie_lock);
 	tmp = readb(s3c_rtc_base + S3C2410_TICNT) & ~S3C2410_TICNT_ENABLE;
 
-	if (to)
+	if (enabled)
 		tmp |= S3C2410_TICNT_ENABLE;
 
 	writeb(tmp, s3c_rtc_base + S3C2410_TICNT);
 	spin_unlock_irq(&s3c_rtc_pie_lock);
+
+	return 0;
 }
 
-static void s3c_rtc_setfreq(int freq)
+static int s3c_rtc_setfreq(struct device *dev, int freq)
 {
 	unsigned int tmp;
 
 	spin_lock_irq(&s3c_rtc_pie_lock);
-	tmp = readb(s3c_rtc_base + S3C2410_TICNT) & S3C2410_TICNT_ENABLE;
-
-	s3c_rtc_freq = freq;
 
+	tmp = readb(s3c_rtc_base + S3C2410_TICNT) & S3C2410_TICNT_ENABLE;
 	tmp |= (128 / freq)-1;
 
 	writeb(tmp, s3c_rtc_base + S3C2410_TICNT);
 	spin_unlock_irq(&s3c_rtc_pie_lock);
+
+	return 0;
 }
 
 /* Time read/write */
@@ -267,12 +267,7 @@ static int s3c_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 
 	writeb(alrm_en, base + S3C2410_RTCALM);
 
-	if (0) {
-		alrm_en = readb(base + S3C2410_RTCALM);
-		alrm_en &= ~S3C2410_RTCALM_ALMEN;
-		writeb(alrm_en, base + S3C2410_RTCALM);
-		disable_irq_wake(s3c_rtc_alarmno);
-	}
+	s3c_rtc_setaie(alrm->enabled);
 
 	if (alrm->enabled)
 		enable_irq_wake(s3c_rtc_alarmno);
@@ -282,59 +277,12 @@ static int s3c_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	return 0;
 }
 
-static int s3c_rtc_ioctl(struct device *dev,
-			 unsigned int cmd, unsigned long arg)
-{
-	unsigned int ret = -ENOIOCTLCMD;
-
-	switch (cmd) {
-	case RTC_AIE_OFF:
-	case RTC_AIE_ON:
-		s3c_rtc_setaie((cmd == RTC_AIE_ON) ? 1 : 0);
-		ret = 0;
-		break;
-
-	case RTC_PIE_OFF:
-	case RTC_PIE_ON:
-		tick_count = 0;
-		s3c_rtc_setpie((cmd == RTC_PIE_ON) ? 1 : 0);
-		ret = 0;
-		break;
-
-	case RTC_IRQP_READ:
-		ret = put_user(s3c_rtc_freq, (unsigned long __user *)arg);
-		break;
-
-	case RTC_IRQP_SET:
-		if (!is_power_of_2(arg)) {
-			ret = -EINVAL;
-			goto exit;
-		}
-
-		pr_debug("s3c2410_rtc: setting frequency %ld\n", arg);
-
-		s3c_rtc_setfreq(arg);
-		ret = 0;
-		break;
-
-	case RTC_UIE_ON:
-	case RTC_UIE_OFF:
-		ret = -EINVAL;
-	}
-
- exit:
-	return ret;
-}
-
 static int s3c_rtc_proc(struct device *dev, struct seq_file *seq)
 {
 	unsigned int ticnt = readb(s3c_rtc_base + S3C2410_TICNT);
 
 	seq_printf(seq, "periodic_IRQ\t: %s\n",
 		     (ticnt & S3C2410_TICNT_ENABLE) ? "yes" : "no" );
-
-	seq_printf(seq, "periodic_freq\t: %d\n", s3c_rtc_freq);
-
 	return 0;
 }
 
@@ -374,7 +322,7 @@ static void s3c_rtc_release(struct device *dev)
 
 	/* do not clear AIE here, it may be needed for wake */
 
-	s3c_rtc_setpie(0);
+	s3c_rtc_setpie(dev, 0);
 	free_irq(s3c_rtc_alarmno, rtc_dev);
 	free_irq(s3c_rtc_tickno, rtc_dev);
 }
@@ -382,11 +330,12 @@ static void s3c_rtc_release(struct device *dev)
 static const struct rtc_class_ops s3c_rtcops = {
 	.open		= s3c_rtc_open,
 	.release	= s3c_rtc_release,
-	.ioctl		= s3c_rtc_ioctl,
 	.read_time	= s3c_rtc_gettime,
 	.set_time	= s3c_rtc_settime,
 	.read_alarm	= s3c_rtc_getalarm,
 	.set_alarm	= s3c_rtc_setalarm,
+	.irq_set_freq	= s3c_rtc_setfreq,
+	.irq_set_state	= s3c_rtc_setpie,
 	.proc	        = s3c_rtc_proc,
 };
 
@@ -430,14 +379,14 @@ static void s3c_rtc_enable(struct platform_device *pdev, int en)
 	}
 }
 
-static int s3c_rtc_remove(struct platform_device *dev)
+static int __devexit s3c_rtc_remove(struct platform_device *dev)
 {
 	struct rtc_device *rtc = platform_get_drvdata(dev);
 
 	platform_set_drvdata(dev, NULL);
 	rtc_device_unregister(rtc);
 
-	s3c_rtc_setpie(0);
+	s3c_rtc_setpie(&dev->dev, 0);
 	s3c_rtc_setaie(0);
 
 	iounmap(s3c_rtc_base);
@@ -447,7 +396,7 @@ static int s3c_rtc_remove(struct platform_device *dev)
 	return 0;
 }
 
-static int s3c_rtc_probe(struct platform_device *pdev)
+static int __devinit s3c_rtc_probe(struct platform_device *pdev)
 {
 	struct rtc_device *rtc;
 	struct resource *res;
@@ -504,7 +453,7 @@ static int s3c_rtc_probe(struct platform_device *pdev)
  	pr_debug("s3c2410_rtc: RTCCON=%02x\n",
 		 readb(s3c_rtc_base + S3C2410_RTCCON));
 
-	s3c_rtc_setfreq(s3c_rtc_freq);
+	s3c_rtc_setfreq(&pdev->dev, 1);
 
 	/* register RTC and exit */
 
@@ -560,7 +509,7 @@ static int s3c_rtc_resume(struct platform_device *pdev)
 
 static struct platform_driver s3c2410_rtcdrv = {
 	.probe		= s3c_rtc_probe,
-	.remove		= s3c_rtc_remove,
+	.remove		= __devexit_p(s3c_rtc_remove),
 	.suspend	= s3c_rtc_suspend,
 	.resume		= s3c_rtc_resume,
 	.driver		= {
diff --git a/drivers/rtc/rtc-vr41xx.c b/drivers/rtc/rtc-vr41xx.c
index be9c70d0b19..884b635f028 100644
--- a/drivers/rtc/rtc-vr41xx.c
+++ b/drivers/rtc/rtc-vr41xx.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for NEC VR4100 series Real Time Clock unit.
  *
- *  Copyright (C) 2003-2006  Yoichi Yuasa <yoichi_yuasa@tripeaks.co.jp>
+ *  Copyright (C) 2003-2008  Yoichi Yuasa <yoichi_yuasa@tripeaks.co.jp>
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -34,7 +34,7 @@
 
 MODULE_AUTHOR("Yoichi Yuasa <yoichi_yuasa@tripeaks.co.jp>");
 MODULE_DESCRIPTION("NEC VR4100 series RTC driver");
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
 
 /* RTC 1 registers */
 #define ETIMELREG		0x00
@@ -82,7 +82,6 @@ static unsigned long epoch = 1970;	/* Jan 1 1970 00:00:00 */
 
 static DEFINE_SPINLOCK(rtc_lock);
 static char rtc_name[] = "RTC";
-static unsigned long periodic_frequency;
 static unsigned long periodic_count;
 static unsigned int alarm_enabled;
 static int aie_irq = -1;
@@ -207,10 +206,37 @@ static int vr41xx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *wkalrm)
 	return 0;
 }
 
-static int vr41xx_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
+static int vr41xx_rtc_irq_set_freq(struct device *dev, int freq)
 {
 	unsigned long count;
 
+	count = RTC_FREQUENCY;
+	do_div(count, freq);
+
+	periodic_count = count;
+
+	spin_lock_irq(&rtc_lock);
+
+	rtc1_write(RTCL1LREG, count);
+	rtc1_write(RTCL1HREG, count >> 16);
+
+	spin_unlock_irq(&rtc_lock);
+
+	return 0;
+}
+
+static int vr41xx_rtc_irq_set_state(struct device *dev, int enabled)
+{
+	if (enabled)
+		enable_irq(pie_irq);
+	else
+		disable_irq(pie_irq);
+
+	return 0;
+}
+
+static int vr41xx_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
+{
 	switch (cmd) {
 	case RTC_AIE_ON:
 		spin_lock_irq(&rtc_lock);
@@ -232,33 +258,6 @@ static int vr41xx_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long
 
 		spin_unlock_irq(&rtc_lock);
 		break;
-	case RTC_PIE_ON:
-		enable_irq(pie_irq);
-		break;
-	case RTC_PIE_OFF:
-		disable_irq(pie_irq);
-		break;
-	case RTC_IRQP_READ:
-		return put_user(periodic_frequency, (unsigned long __user *)arg);
-		break;
-	case RTC_IRQP_SET:
-		if (arg > MAX_PERIODIC_RATE)
-			return -EINVAL;
-
-		periodic_frequency = arg;
-
-		count = RTC_FREQUENCY;
-		do_div(count, arg);
-
-		periodic_count = count;
-
-		spin_lock_irq(&rtc_lock);
-
-		rtc1_write(RTCL1LREG, count);
-		rtc1_write(RTCL1HREG, count >> 16);
-
-		spin_unlock_irq(&rtc_lock);
-		break;
 	case RTC_EPOCH_READ:
 		return put_user(epoch, (unsigned long __user *)arg);
 	case RTC_EPOCH_SET:
@@ -309,6 +308,8 @@ static const struct rtc_class_ops vr41xx_rtc_ops = {
 	.set_time	= vr41xx_rtc_set_time,
 	.read_alarm	= vr41xx_rtc_read_alarm,
 	.set_alarm	= vr41xx_rtc_set_alarm,
+	.irq_set_freq	= vr41xx_rtc_irq_set_freq,
+	.irq_set_state	= vr41xx_rtc_irq_set_state,
 };
 
 static int __devinit rtc_probe(struct platform_device *pdev)
@@ -346,6 +347,8 @@ static int __devinit rtc_probe(struct platform_device *pdev)
 		goto err_iounmap_all;
 	}
 
+	rtc->max_user_freq = MAX_PERIODIC_RATE;
+
 	spin_lock_irq(&rtc_lock);
 
 	rtc1_write(ECMPLREG, 0);
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index 5ab34340919..79954bd6bfa 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -15,6 +15,7 @@
 #include <linux/err.h>
 #include <linux/virtio.h>
 #include <linux/virtio_config.h>
+#include <linux/virtio_console.h>
 #include <linux/interrupt.h>
 #include <linux/virtio_ring.h>
 #include <linux/pfn.h>
@@ -87,16 +88,20 @@ static u32 kvm_get_features(struct virtio_device *vdev)
 	return features;
 }
 
-static void kvm_set_features(struct virtio_device *vdev, u32 features)
+static void kvm_finalize_features(struct virtio_device *vdev)
 {
-	unsigned int i;
+	unsigned int i, bits;
 	struct kvm_device_desc *desc = to_kvmdev(vdev)->desc;
 	/* Second half of bitmap is features we accept. */
 	u8 *out_features = kvm_vq_features(desc) + desc->feature_len;
 
+	/* Give virtio_ring a chance to accept features. */
+	vring_transport_features(vdev);
+
 	memset(out_features, 0, desc->feature_len);
-	for (i = 0; i < min(desc->feature_len * 8, 32); i++) {
-		if (features & (1 << i))
+	bits = min_t(unsigned, desc->feature_len, sizeof(vdev->features)) * 8;
+	for (i = 0; i < bits; i++) {
+		if (test_bit(i, vdev->features))
 			out_features[i / 8] |= (1 << (i % 8));
 	}
 }
@@ -222,7 +227,7 @@ static void kvm_del_vq(struct virtqueue *vq)
  */
 static struct virtio_config_ops kvm_vq_configspace_ops = {
 	.get_features = kvm_get_features,
-	.set_features = kvm_set_features,
+	.finalize_features = kvm_finalize_features,
 	.get = kvm_get,
 	.set = kvm_set,
 	.get_status = kvm_get_status,
@@ -333,6 +338,25 @@ static int __init kvm_devices_init(void)
 	return 0;
 }
 
+/* code for early console output with virtio_console */
+static __init int early_put_chars(u32 vtermno, const char *buf, int count)
+{
+	char scratch[17];
+	unsigned int len = count;
+
+	if (len > sizeof(scratch) - 1)
+		len = sizeof(scratch) - 1;
+	scratch[len] = '\0';
+	memcpy(scratch, buf, len);
+	kvm_hypercall1(KVM_S390_VIRTIO_NOTIFY, __pa(scratch));
+	return len;
+}
+
+void s390_virtio_console_init(void)
+{
+	virtio_cons_early_init(early_put_chars);
+}
+
 /*
  * We do this after core stuff, but before the drivers.
  */
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index c3ad89e302b..cebb25e36e8 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -3321,7 +3321,7 @@ int qeth_change_mtu(struct net_device *dev, int new_mtu)
 	struct qeth_card *card;
 	char dbf_text[15];
 
-	card = netdev_priv(dev);
+	card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 4, "chgmtu");
 	sprintf(dbf_text, "%8x", new_mtu);
@@ -3343,7 +3343,7 @@ struct net_device_stats *qeth_get_stats(struct net_device *dev)
 {
 	struct qeth_card *card;
 
-	card = netdev_priv(dev);
+	card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 5, "getstat");
 
@@ -3395,7 +3395,7 @@ void qeth_tx_timeout(struct net_device *dev)
 {
 	struct qeth_card *card;
 
-	card = netdev_priv(dev);
+	card = dev->ml_priv;
 	card->stats.tx_errors++;
 	qeth_schedule_recovery(card);
 }
@@ -3403,7 +3403,7 @@ EXPORT_SYMBOL_GPL(qeth_tx_timeout);
 
 int qeth_mdio_read(struct net_device *dev, int phy_id, int regnum)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	int rc = 0;
 
 	switch (regnum) {
@@ -4253,7 +4253,7 @@ EXPORT_SYMBOL_GPL(qeth_core_get_stats_count);
 void qeth_core_get_ethtool_stats(struct net_device *dev,
 		struct ethtool_stats *stats, u64 *data)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	data[0] = card->stats.rx_packets -
 				card->perf_stats.initial_rx_packets;
 	data[1] = card->perf_stats.bufs_rec;
@@ -4313,7 +4313,7 @@ EXPORT_SYMBOL_GPL(qeth_core_get_strings);
 void qeth_core_get_drvinfo(struct net_device *dev,
 		struct ethtool_drvinfo *info)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	if (card->options.layer2)
 		strcpy(info->driver, "qeth_l2");
 	else
@@ -4331,7 +4331,7 @@ EXPORT_SYMBOL_GPL(qeth_core_get_drvinfo);
 int qeth_core_ethtool_get_settings(struct net_device *netdev,
 					struct ethtool_cmd *ecmd)
 {
-	struct qeth_card *card = netdev_priv(netdev);
+	struct qeth_card *card = netdev->ml_priv;
 	enum qeth_link_types link_type;
 
 	if ((card->info.type == QETH_CARD_TYPE_IQD) || (card->info.guestlan))
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 3fbc3bdec0c..a8b069cd9a4 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -35,7 +35,7 @@ static int qeth_l2_recover(void *);
 
 static int qeth_l2_do_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct mii_ioctl_data *mii_data;
 	int rc = 0;
 
@@ -317,7 +317,7 @@ static void qeth_l2_process_vlans(struct qeth_card *card, int clear)
 
 static void qeth_l2_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct qeth_vlan_vid *id;
 
 	QETH_DBF_TEXT_(TRACE, 4, "aid:%d", vid);
@@ -334,7 +334,7 @@ static void qeth_l2_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 static void qeth_l2_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
 {
 	struct qeth_vlan_vid *id, *tmpid = NULL;
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT_(TRACE, 4, "kid:%d", vid);
 	spin_lock_bh(&card->vlanlock);
@@ -566,7 +566,7 @@ static int qeth_l2_request_initial_mac(struct qeth_card *card)
 static int qeth_l2_set_mac_address(struct net_device *dev, void *p)
 {
 	struct sockaddr *addr = p;
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	int rc = 0;
 
 	QETH_DBF_TEXT(TRACE, 3, "setmac");
@@ -590,7 +590,7 @@ static int qeth_l2_set_mac_address(struct net_device *dev, void *p)
 
 static void qeth_l2_set_multicast_list(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct dev_mc_list *dm;
 
 	if (card->info.type == QETH_CARD_TYPE_OSN)
@@ -612,7 +612,7 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	int rc;
 	struct qeth_hdr *hdr = NULL;
 	int elements = 0;
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct sk_buff *new_skb = skb;
 	int ipv = qeth_get_ip_version(skb);
 	int cast_type = qeth_get_cast_type(card, skb);
@@ -767,7 +767,7 @@ static void qeth_l2_qdio_input_handler(struct ccw_device *ccwdev,
 
 static int qeth_l2_open(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 4, "qethopen");
 	if (card->state != CARD_STATE_SOFTSETUP)
@@ -791,7 +791,7 @@ static int qeth_l2_open(struct net_device *dev)
 
 static int qeth_l2_stop(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 4, "qethstop");
 	netif_tx_disable(dev);
@@ -838,7 +838,7 @@ static void qeth_l2_remove_device(struct ccwgroup_device *cgdev)
 
 static int qeth_l2_ethtool_set_tso(struct net_device *dev, u32 data)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	if (data) {
 		if (card->options.large_send == QETH_LARGE_SEND_NO) {
@@ -894,7 +894,7 @@ static int qeth_l2_setup_netdev(struct qeth_card *card)
 	if (!card->dev)
 		return -ENODEV;
 
-	card->dev->priv = card;
+	card->dev->ml_priv = card;
 	card->dev->tx_timeout = &qeth_tx_timeout;
 	card->dev->watchdog_timeo = QETH_TX_TIMEOUT;
 	card->dev->open = qeth_l2_open;
@@ -1178,7 +1178,7 @@ int qeth_osn_assist(struct net_device *dev, void *data, int data_len)
 	QETH_DBF_TEXT(TRACE, 2, "osnsdmc");
 	if (!dev)
 		return -ENODEV;
-	card = netdev_priv(dev);
+	card = dev->ml_priv;
 	if (!card)
 		return -ENODEV;
 	if ((card->state != CARD_STATE_UP) &&
@@ -1201,7 +1201,7 @@ int qeth_osn_register(unsigned char *read_dev_no, struct net_device **dev,
 	*dev = qeth_l2_netdev_by_devno(read_dev_no);
 	if (*dev == NULL)
 		return -ENODEV;
-	card = netdev_priv(*dev);
+	card = (*dev)->ml_priv;
 	if (!card)
 		return -ENODEV;
 	if ((assist_cb == NULL) || (data_cb == NULL))
@@ -1219,7 +1219,7 @@ void qeth_osn_deregister(struct net_device *dev)
 	QETH_DBF_TEXT(TRACE, 2, "osndereg");
 	if (!dev)
 		return;
-	card = netdev_priv(dev);
+	card = dev->ml_priv;
 	if (!card)
 		return;
 	card->osn_info.assist_cb = NULL;
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 38de31b5570..3e1d1385735 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -1813,7 +1813,7 @@ static void qeth_l3_free_vlan_addresses(struct qeth_card *card,
 static void qeth_l3_vlan_rx_register(struct net_device *dev,
 			struct vlan_group *grp)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	unsigned long flags;
 
 	QETH_DBF_TEXT(TRACE, 4, "vlanreg");
@@ -1825,7 +1825,7 @@ static void qeth_l3_vlan_rx_register(struct net_device *dev,
 static void qeth_l3_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 {
 	struct net_device *vlandev;
-	struct qeth_card *card = (struct qeth_card *) dev->priv;
+	struct qeth_card *card = dev->ml_priv;
 	struct in_device *in_dev;
 
 	if (card->info.type == QETH_CARD_TYPE_IQD)
@@ -1851,7 +1851,7 @@ static void qeth_l3_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 
 static void qeth_l3_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	unsigned long flags;
 
 	QETH_DBF_TEXT_(TRACE, 4, "kid:%d", vid);
@@ -2013,7 +2013,7 @@ static int qeth_l3_verify_vlan_dev(struct net_device *dev,
 		}
 	}
 
-	if (rc && !(netdev_priv(vlan_dev_real_dev(dev)) == (void *)card))
+	if (rc && !(vlan_dev_real_dev(dev)->ml_priv == (void *)card))
 		return 0;
 
 	return rc;
@@ -2047,9 +2047,9 @@ static struct qeth_card *qeth_l3_get_card_from_dev(struct net_device *dev)
 
 	rc = qeth_l3_verify_dev(dev);
 	if (rc == QETH_REAL_CARD)
-		card = netdev_priv(dev);
+		card = dev->ml_priv;
 	else if (rc == QETH_VLAN_CARD)
-		card = netdev_priv(vlan_dev_real_dev(dev));
+		card = vlan_dev_real_dev(dev)->ml_priv;
 	if (card && card->options.layer2)
 		card = NULL;
 	QETH_DBF_TEXT_(TRACE, 4, "%d", rc);
@@ -2110,7 +2110,7 @@ static int qeth_l3_stop_card(struct qeth_card *card, int recovery_mode)
 
 static void qeth_l3_set_multicast_list(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 3, "setmulti");
 	qeth_l3_delete_mc_addresses(card);
@@ -2438,7 +2438,7 @@ static int qeth_l3_arp_flush_cache(struct qeth_card *card)
 
 static int qeth_l3_do_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct qeth_arp_cache_entry arp_entry;
 	struct mii_ioctl_data *mii_data;
 	int rc = 0;
@@ -2595,7 +2595,7 @@ static int qeth_l3_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	u16 *tag;
 	struct qeth_hdr *hdr = NULL;
 	int elements_needed = 0;
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	struct sk_buff *new_skb = NULL;
 	int ipv = qeth_get_ip_version(skb);
 	int cast_type = qeth_get_cast_type(card, skb);
@@ -2763,7 +2763,7 @@ tx_drop:
 
 static int qeth_l3_open(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 4, "qethopen");
 	if (card->state != CARD_STATE_SOFTSETUP)
@@ -2780,7 +2780,7 @@ static int qeth_l3_open(struct net_device *dev)
 
 static int qeth_l3_stop(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	QETH_DBF_TEXT(TRACE, 4, "qethstop");
 	netif_tx_disable(dev);
@@ -2792,14 +2792,14 @@ static int qeth_l3_stop(struct net_device *dev)
 
 static u32 qeth_l3_ethtool_get_rx_csum(struct net_device *dev)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	return (card->options.checksum_type == HW_CHECKSUMMING);
 }
 
 static int qeth_l3_ethtool_set_rx_csum(struct net_device *dev, u32 data)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 	enum qeth_card_states old_state;
 	enum qeth_checksum_types csum_type;
 
@@ -2825,7 +2825,7 @@ static int qeth_l3_ethtool_set_rx_csum(struct net_device *dev, u32 data)
 
 static int qeth_l3_ethtool_set_tso(struct net_device *dev, u32 data)
 {
-	struct qeth_card *card = netdev_priv(dev);
+	struct qeth_card *card = dev->ml_priv;
 
 	if (data) {
 		if (card->options.large_send == QETH_LARGE_SEND_NO) {
@@ -2915,7 +2915,7 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 		return -ENODEV;
 
 	card->dev->hard_start_xmit = qeth_l3_hard_start_xmit;
-	card->dev->priv = card;
+	card->dev->ml_priv = card;
 	card->dev->tx_timeout = &qeth_tx_timeout;
 	card->dev->watchdog_timeo = QETH_TX_TIMEOUT;
 	card->dev->open = qeth_l3_open;
diff --git a/drivers/scsi/ibmvscsi/ibmvfc.c b/drivers/scsi/ibmvscsi/ibmvfc.c
index eb702b96d57..61f8fdea2d9 100644
--- a/drivers/scsi/ibmvscsi/ibmvfc.c
+++ b/drivers/scsi/ibmvscsi/ibmvfc.c
@@ -3525,7 +3525,7 @@ static int ibmvfc_init_crq(struct ibmvfc_host *vhost)
 	crq->msg_token = dma_map_single(dev, crq->msgs,
 					PAGE_SIZE, DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(crq->msg_token))
+	if (dma_mapping_error(dev, crq->msg_token))
 		goto map_failed;
 
 	retrc = rc = plpar_hcall_norets(H_REG_CRQ, vdev->unit_address,
@@ -3618,7 +3618,7 @@ static int ibmvfc_alloc_mem(struct ibmvfc_host *vhost)
 					    async_q->size * sizeof(*async_q->msgs),
 					    DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(async_q->msg_token)) {
+	if (dma_mapping_error(dev, async_q->msg_token)) {
 		dev_err(dev, "Failed to map async queue\n");
 		goto free_async_crq;
 	}
@@ -3819,6 +3819,20 @@ static int ibmvfc_remove(struct vio_dev *vdev)
 	return 0;
 }
 
+/**
+ * ibmvfc_get_desired_dma - Calculate DMA resources needed by the driver
+ * @vdev:	vio device struct
+ *
+ * Return value:
+ *	Number of bytes the driver will need to DMA map at the same time in
+ *	order to perform well.
+ */
+static unsigned long ibmvfc_get_desired_dma(struct vio_dev *vdev)
+{
+	unsigned long pool_dma = max_requests * sizeof(union ibmvfc_iu);
+	return pool_dma + ((512 * 1024) * driver_template.cmd_per_lun);
+}
+
 static struct vio_device_id ibmvfc_device_table[] __devinitdata = {
 	{"fcp", "IBM,vfc-client"},
 	{ "", "" }
@@ -3829,6 +3843,7 @@ static struct vio_driver ibmvfc_driver = {
 	.id_table = ibmvfc_device_table,
 	.probe = ibmvfc_probe,
 	.remove = ibmvfc_remove,
+	.get_desired_dma = ibmvfc_get_desired_dma,
 	.driver = {
 		.name = IBMVFC_NAME,
 		.owner = THIS_MODULE,
diff --git a/drivers/scsi/ibmvscsi/ibmvscsi.c b/drivers/scsi/ibmvscsi/ibmvscsi.c
index 5d23368a1bc..6b24b9cdb04 100644
--- a/drivers/scsi/ibmvscsi/ibmvscsi.c
+++ b/drivers/scsi/ibmvscsi/ibmvscsi.c
@@ -72,6 +72,7 @@
 #include <linux/delay.h>
 #include <asm/firmware.h>
 #include <asm/vio.h>
+#include <asm/firmware.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_host.h>
@@ -426,8 +427,10 @@ static int map_sg_data(struct scsi_cmnd *cmd,
 					   SG_ALL * sizeof(struct srp_direct_buf),
 					   &evt_struct->ext_list_token, 0);
 		if (!evt_struct->ext_list) {
-			sdev_printk(KERN_ERR, cmd->device,
-				    "Can't allocate memory for indirect table\n");
+			if (!firmware_has_feature(FW_FEATURE_CMO))
+				sdev_printk(KERN_ERR, cmd->device,
+				            "Can't allocate memory "
+				            "for indirect table\n");
 			return 0;
 		}
 	}
@@ -743,7 +746,9 @@ static int ibmvscsi_queuecommand(struct scsi_cmnd *cmnd,
 	srp_cmd->lun = ((u64) lun) << 48;
 
 	if (!map_data_for_srp_cmd(cmnd, evt_struct, srp_cmd, hostdata->dev)) {
-		sdev_printk(KERN_ERR, cmnd->device, "couldn't convert cmd to srp_cmd\n");
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			sdev_printk(KERN_ERR, cmnd->device,
+			            "couldn't convert cmd to srp_cmd\n");
 		free_event_struct(&hostdata->pool, evt_struct);
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
@@ -854,8 +859,11 @@ static void send_mad_adapter_info(struct ibmvscsi_host_data *hostdata)
 					    sizeof(hostdata->madapter_info),
 					    DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(req->buffer)) {
-		dev_err(hostdata->dev, "Unable to map request_buffer for adapter_info!\n");
+	if (dma_mapping_error(hostdata->dev, req->buffer)) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			dev_err(hostdata->dev,
+			        "Unable to map request_buffer for "
+			        "adapter_info!\n");
 		free_event_struct(&hostdata->pool, evt_struct);
 		return;
 	}
@@ -1399,8 +1407,10 @@ static int ibmvscsi_do_host_config(struct ibmvscsi_host_data *hostdata,
 						    length,
 						    DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(host_config->buffer)) {
-		dev_err(hostdata->dev, "dma_mapping error getting host config\n");
+	if (dma_mapping_error(hostdata->dev, host_config->buffer)) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			dev_err(hostdata->dev,
+			        "dma_mapping error getting host config\n");
 		free_event_struct(&hostdata->pool, evt_struct);
 		return -1;
 	}
@@ -1604,7 +1614,7 @@ static struct scsi_host_template driver_template = {
 	.eh_host_reset_handler = ibmvscsi_eh_host_reset_handler,
 	.slave_configure = ibmvscsi_slave_configure,
 	.change_queue_depth = ibmvscsi_change_queue_depth,
-	.cmd_per_lun = 16,
+	.cmd_per_lun = IBMVSCSI_CMDS_PER_LUN_DEFAULT,
 	.can_queue = IBMVSCSI_MAX_REQUESTS_DEFAULT,
 	.this_id = -1,
 	.sg_tablesize = SG_ALL,
@@ -1613,6 +1623,26 @@ static struct scsi_host_template driver_template = {
 };
 
 /**
+ * ibmvscsi_get_desired_dma - Calculate IO memory desired by the driver
+ *
+ * @vdev: struct vio_dev for the device whose desired IO mem is to be returned
+ *
+ * Return value:
+ *	Number of bytes of IO data the driver will need to perform well.
+ */
+static unsigned long ibmvscsi_get_desired_dma(struct vio_dev *vdev)
+{
+	/* iu_storage data allocated in initialize_event_pool */
+	unsigned long desired_io = max_requests * sizeof(union viosrp_iu);
+
+	/* add io space for sg data */
+	desired_io += (IBMVSCSI_MAX_SECTORS_DEFAULT *
+	                     IBMVSCSI_CMDS_PER_LUN_DEFAULT);
+
+	return desired_io;
+}
+
+/**
  * Called by bus code for each adapter
  */
 static int ibmvscsi_probe(struct vio_dev *vdev, const struct vio_device_id *id)
@@ -1641,7 +1671,7 @@ static int ibmvscsi_probe(struct vio_dev *vdev, const struct vio_device_id *id)
 	hostdata->host = host;
 	hostdata->dev = dev;
 	atomic_set(&hostdata->request_limit, -1);
-	hostdata->host->max_sectors = 32 * 8; /* default max I/O 32 pages */
+	hostdata->host->max_sectors = IBMVSCSI_MAX_SECTORS_DEFAULT;
 
 	rc = ibmvscsi_ops->init_crq_queue(&hostdata->queue, hostdata, max_requests);
 	if (rc != 0 && rc != H_RESOURCE) {
@@ -1735,6 +1765,7 @@ static struct vio_driver ibmvscsi_driver = {
 	.id_table = ibmvscsi_device_table,
 	.probe = ibmvscsi_probe,
 	.remove = ibmvscsi_remove,
+	.get_desired_dma = ibmvscsi_get_desired_dma,
 	.driver = {
 		.name = "ibmvscsi",
 		.owner = THIS_MODULE,
diff --git a/drivers/scsi/ibmvscsi/ibmvscsi.h b/drivers/scsi/ibmvscsi/ibmvscsi.h
index 46e850e302c..2d4339d5e16 100644
--- a/drivers/scsi/ibmvscsi/ibmvscsi.h
+++ b/drivers/scsi/ibmvscsi/ibmvscsi.h
@@ -45,6 +45,8 @@ struct Scsi_Host;
 #define MAX_INDIRECT_BUFS 10
 
 #define IBMVSCSI_MAX_REQUESTS_DEFAULT 100
+#define IBMVSCSI_CMDS_PER_LUN_DEFAULT 16
+#define IBMVSCSI_MAX_SECTORS_DEFAULT 256 /* 32 * 8 = default max I/O 32 pages */
 #define IBMVSCSI_MAX_CMDS_PER_LUN 64
 
 /* ------------------------------------------------------------
diff --git a/drivers/scsi/ibmvscsi/ibmvstgt.c b/drivers/scsi/ibmvscsi/ibmvstgt.c
index 3b9514c8f1f..2e13ec00172 100644
--- a/drivers/scsi/ibmvscsi/ibmvstgt.c
+++ b/drivers/scsi/ibmvscsi/ibmvstgt.c
@@ -564,7 +564,7 @@ static int crq_queue_create(struct crq_queue *queue, struct srp_target *target)
 					  queue->size * sizeof(*queue->msgs),
 					  DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(queue->msg_token))
+	if (dma_mapping_error(target->dev, queue->msg_token))
 		goto map_failed;
 
 	err = h_reg_crq(vport->dma_dev->unit_address, queue->msg_token,
diff --git a/drivers/scsi/ibmvscsi/rpa_vscsi.c b/drivers/scsi/ibmvscsi/rpa_vscsi.c
index 182146100dc..462a8574dad 100644
--- a/drivers/scsi/ibmvscsi/rpa_vscsi.c
+++ b/drivers/scsi/ibmvscsi/rpa_vscsi.c
@@ -253,7 +253,7 @@ static int rpavscsi_init_crq_queue(struct crq_queue *queue,
 					  queue->size * sizeof(*queue->msgs),
 					  DMA_BIDIRECTIONAL);
 
-	if (dma_mapping_error(queue->msg_token))
+	if (dma_mapping_error(hostdata->dev, queue->msg_token))
 		goto map_failed;
 
 	gather_partition_info();
diff --git a/drivers/scsi/ide-scsi.c b/drivers/scsi/ide-scsi.c
index 538552495d4..b40a673985a 100644
--- a/drivers/scsi/ide-scsi.c
+++ b/drivers/scsi/ide-scsi.c
@@ -101,8 +101,13 @@ static struct ide_scsi_obj *ide_scsi_get(struct gendisk *disk)
 
 	mutex_lock(&idescsi_ref_mutex);
 	scsi = ide_scsi_g(disk);
-	if (scsi)
+	if (scsi) {
 		scsi_host_get(scsi->host);
+		if (ide_device_get(scsi->drive)) {
+			scsi_host_put(scsi->host);
+			scsi = NULL;
+		}
+	}
 	mutex_unlock(&idescsi_ref_mutex);
 	return scsi;
 }
@@ -110,6 +115,7 @@ static struct ide_scsi_obj *ide_scsi_get(struct gendisk *disk)
 static void ide_scsi_put(struct ide_scsi_obj *scsi)
 {
 	mutex_lock(&idescsi_ref_mutex);
+	ide_device_put(scsi->drive);
 	scsi_host_put(scsi->host);
 	mutex_unlock(&idescsi_ref_mutex);
 }
@@ -201,15 +207,15 @@ static int idescsi_check_condition(ide_drive_t *drive,
 
 	/* stuff a sense request in front of our current request */
 	pc = kzalloc(sizeof(struct ide_atapi_pc), GFP_ATOMIC);
-	rq = kmalloc(sizeof(struct request), GFP_ATOMIC);
+	rq = blk_get_request(drive->queue, READ, GFP_ATOMIC);
 	buf = kzalloc(SCSI_SENSE_BUFFERSIZE, GFP_ATOMIC);
 	if (!pc || !rq || !buf) {
 		kfree(buf);
-		kfree(rq);
+		if (rq)
+			blk_put_request(rq);
 		kfree(pc);
 		return -ENOMEM;
 	}
-	blk_rq_init(NULL, rq);
 	rq->special = (char *) pc;
 	pc->rq = rq;
 	pc->buf = buf;
@@ -226,6 +232,7 @@ static int idescsi_check_condition(ide_drive_t *drive,
 		ide_scsi_hex_dump(pc->c, 6);
 	}
 	rq->rq_disk = scsi->disk;
+	rq->ref_count++;
 	memcpy(rq->cmd, pc->c, 12);
 	ide_do_drive_cmd(drive, rq);
 	return 0;
@@ -272,7 +279,7 @@ static int idescsi_end_request (ide_drive_t *drive, int uptodate, int nrsecs)
 			SCSI_SENSE_BUFFERSIZE);
 		kfree(pc->buf);
 		kfree(pc);
-		kfree(rq);
+		blk_put_request(rq);
 		pc = opc;
 		rq = pc->rq;
 		pc->scsi_cmd->result = (CHECK_CONDITION << 1) |
@@ -303,7 +310,7 @@ static int idescsi_end_request (ide_drive_t *drive, int uptodate, int nrsecs)
 	pc->done(pc->scsi_cmd);
 	spin_unlock_irqrestore(host->host_lock, flags);
 	kfree(pc);
-	kfree(rq);
+	blk_put_request(rq);
 	scsi->pc = NULL;
 	return 0;
 }
@@ -577,6 +584,7 @@ static int idescsi_queue (struct scsi_cmnd *cmd,
 	ide_drive_t *drive = scsi->drive;
 	struct request *rq = NULL;
 	struct ide_atapi_pc *pc = NULL;
+	int write = cmd->sc_data_direction == DMA_TO_DEVICE;
 
 	if (!drive) {
 		scmd_printk (KERN_ERR, cmd, "drive not present\n");
@@ -584,7 +592,7 @@ static int idescsi_queue (struct scsi_cmnd *cmd,
 	}
 	scsi = drive_to_idescsi(drive);
 	pc = kmalloc(sizeof(struct ide_atapi_pc), GFP_ATOMIC);
-	rq = kmalloc(sizeof(struct request), GFP_ATOMIC);
+	rq = blk_get_request(drive->queue, write, GFP_ATOMIC);
 	if (rq == NULL || pc == NULL) {
 		printk (KERN_ERR "ide-scsi: %s: out of memory\n", drive->name);
 		goto abort;
@@ -614,17 +622,18 @@ static int idescsi_queue (struct scsi_cmnd *cmd,
 		}
 	}
 
-	blk_rq_init(NULL, rq);
 	rq->special = (char *) pc;
 	rq->cmd_type = REQ_TYPE_SPECIAL;
 	spin_unlock_irq(host->host_lock);
+	rq->ref_count++;
 	memcpy(rq->cmd, pc->c, 12);
 	blk_execute_rq_nowait(drive->queue, scsi->disk, rq, 0, NULL);
 	spin_lock_irq(host->host_lock);
 	return 0;
 abort:
 	kfree (pc);
-	kfree (rq);
+	if (rq)
+		blk_put_request(rq);
 	cmd->result = DID_ERROR << 16;
 	done(cmd);
 	return 0;
@@ -672,7 +681,9 @@ static int idescsi_eh_abort (struct scsi_cmnd *cmd)
 
 		if (blk_sense_request(scsi->pc->rq))
 			kfree(scsi->pc->buf);
-		kfree(scsi->pc->rq);
+		/* we need to call blk_put_request twice. */
+		blk_put_request(scsi->pc->rq);
+		blk_put_request(scsi->pc->rq);
 		kfree(scsi->pc);
 		scsi->pc = NULL;
 
@@ -724,7 +735,7 @@ static int idescsi_eh_reset (struct scsi_cmnd *cmd)
 		kfree(scsi->pc->buf);
 	kfree(scsi->pc);
 	scsi->pc = NULL;
-	kfree(req);
+	blk_put_request(req);
 
 	/* now nuke the drive queue */
 	while ((req = elv_next_request(drive->queue))) {
diff --git a/drivers/scsi/sun_esp.c b/drivers/scsi/sun_esp.c
index 2c87db98cdf..f9cf7015136 100644
--- a/drivers/scsi/sun_esp.c
+++ b/drivers/scsi/sun_esp.c
@@ -7,6 +7,7 @@
 #include <linux/types.h>
 #include <linux/delay.h>
 #include <linux/module.h>
+#include <linux/mm.h>
 #include <linux/init.h>
 
 #include <asm/irq.h>
diff --git a/drivers/serial/8250.c b/drivers/serial/8250.c
index 27f34a9f9cb..a97f1ae11f7 100644
--- a/drivers/serial/8250.c
+++ b/drivers/serial/8250.c
@@ -1293,7 +1293,18 @@ receive_chars(struct uart_8250_port *up, unsigned int *status)
 	char flag;
 
 	do {
-		ch = serial_inp(up, UART_RX);
+		if (likely(lsr & UART_LSR_DR))
+			ch = serial_inp(up, UART_RX);
+		else
+			/*
+			 * Intel 82571 has a Serial Over Lan device that will
+			 * set UART_LSR_BI without setting UART_LSR_DR when
+			 * it receives a break. To avoid reading from the
+			 * receive buffer without UART_LSR_DR bit set, we
+			 * just force the read character to be 0
+			 */
+			ch = 0;
+
 		flag = TTY_NORMAL;
 		up->port.icount.rx++;
 
@@ -1342,7 +1353,7 @@ receive_chars(struct uart_8250_port *up, unsigned int *status)
 
 ignore_char:
 		lsr = serial_inp(up, UART_LSR);
-	} while ((lsr & UART_LSR_DR) && (max_count-- > 0));
+	} 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);
@@ -1425,7 +1436,7 @@ serial8250_handle_port(struct uart_8250_port *up)
 
 	DEBUG_INTR("status = %x...", status);
 
-	if (status & UART_LSR_DR)
+	if (status & (UART_LSR_DR | UART_LSR_BI))
 		receive_chars(up, &status);
 	check_modem_status(up);
 	if (status & UART_LSR_THRE)
diff --git a/drivers/serial/8250_gsc.c b/drivers/serial/8250_gsc.c
index 4eb7437a404..0416ad3bc12 100644
--- a/drivers/serial/8250_gsc.c
+++ b/drivers/serial/8250_gsc.c
@@ -119,3 +119,5 @@ int __init probe_serial_gsc(void)
 }
 
 module_init(probe_serial_gsc);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/serial/8250_pci.c b/drivers/serial/8250_pci.c
index 1b36087665a..c2f23933155 100644
--- a/drivers/serial/8250_pci.c
+++ b/drivers/serial/8250_pci.c
@@ -767,6 +767,9 @@ pci_default_setup(struct serial_private *priv, struct pciserial_board *board,
 #define PCI_SUBDEVICE_ID_POCTAL232	0x0308
 #define PCI_SUBDEVICE_ID_POCTAL422	0x0408
 
+/* Unknown vendors/cards - this should not be in linux/pci_ids.h */
+#define PCI_SUBDEVICE_ID_UNKNOWN_0x1584	0x1584
+
 /*
  * Master list of serial port init/setup/exit quirks.
  * This does not describe the general nature of the port.
@@ -882,6 +885,15 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 	},
 	{
 		.vendor		= PCI_VENDOR_ID_PLX,
+		.device		= PCI_DEVICE_ID_PLX_9050,
+		.subvendor	= PCI_VENDOR_ID_PLX,
+		.subdevice	= PCI_SUBDEVICE_ID_UNKNOWN_0x1584,
+		.init		= pci_plx9050_init,
+		.setup		= pci_default_setup,
+		.exit		= __devexit_p(pci_plx9050_exit),
+	},
+	{
+		.vendor		= PCI_VENDOR_ID_PLX,
 		.device		= PCI_DEVICE_ID_PLX_ROMULUS,
 		.subvendor	= PCI_VENDOR_ID_PLX,
 		.subdevice	= PCI_DEVICE_ID_PLX_ROMULUS,
@@ -2197,6 +2209,11 @@ static struct pci_device_id serial_pci_tbl[] = {
 	{	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_1077,
 		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
 		pbn_b2_4_921600 },
+	/* Unknown card - subdevice 0x1584 */
+	{	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050,
+		PCI_VENDOR_ID_PLX,
+		PCI_SUBDEVICE_ID_UNKNOWN_0x1584, 0, 0,
+		pbn_b0_4_115200 },
 	{	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050,
 		PCI_SUBVENDOR_ID_KEYSPAN,
 		PCI_SUBDEVICE_ID_KEYSPAN_SX2, 0, 0,
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 8fc7451c004..3b4a14e355c 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -942,22 +942,6 @@ config SERIAL_IP22_ZILOG_CONSOLE
 	depends on SERIAL_IP22_ZILOG=y
 	select SERIAL_CORE_CONSOLE
 
-config V850E_UART
-	bool "NEC V850E on-chip UART support"
-	depends on V850E_MA1 || V850E_ME2 || V850E_TEG || V850E2_ANNA || V850E_AS85EP1
-	select SERIAL_CORE
-	default y
-
-config V850E_UARTB
-        bool
-	depends on V850E_UART && V850E_ME2
-	default y
-
-config V850E_UART_CONSOLE
-	bool "Use NEC V850E on-chip UART for console"
-	depends on V850E_UART
-	select SERIAL_CORE_CONSOLE
-
 config SERIAL_SH_SCI
 	tristate "SuperH SCI(F) serial port support"
 	depends on SUPERH || H8300
diff --git a/drivers/serial/cpm_uart/cpm_uart_core.c b/drivers/serial/cpm_uart/cpm_uart_core.c
index 1ff80de177d..a4f86927a74 100644
--- a/drivers/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/serial/cpm_uart/cpm_uart_core.c
@@ -435,10 +435,13 @@ static void cpm_uart_shutdown(struct uart_port *port)
 		}
 
 		/* Shut them really down and reinit buffer descriptors */
-		if (IS_SMC(pinfo))
+		if (IS_SMC(pinfo)) {
+			out_be16(&pinfo->smcup->smc_brkcr, 0);
 			cpm_line_cr_cmd(pinfo, CPM_CR_STOP_TX);
-		else
+		} else {
+			out_be16(&pinfo->sccup->scc_brkcr, 0);
 			cpm_line_cr_cmd(pinfo, CPM_CR_GRA_STOP_TX);
+		}
 
 		cpm_uart_initbd(pinfo);
 	}
@@ -554,9 +557,11 @@ static void cpm_uart_set_termios(struct uart_port *port,
 		 * enables, because we want to put them back if they were
 		 * present.
 		 */
-		prev_mode = in_be16(&smcp->smc_smcmr);
-		out_be16(&smcp->smc_smcmr, smcr_mk_clen(bits) | cval | SMCMR_SM_UART);
-		setbits16(&smcp->smc_smcmr, (prev_mode & (SMCMR_REN | SMCMR_TEN)));
+		prev_mode = in_be16(&smcp->smc_smcmr) & (SMCMR_REN | SMCMR_TEN);
+		/* Output in *one* operation, so we don't interrupt RX/TX if they
+		 * were already enabled. */
+		out_be16(&smcp->smc_smcmr, smcr_mk_clen(bits) | cval |
+		    SMCMR_SM_UART | prev_mode);
 	} else {
 		out_be16(&sccp->scc_psmr, (sbits << 12) | scval);
 	}
@@ -1198,12 +1203,14 @@ static int __init cpm_uart_console_setup(struct console *co, char *options)
 	udbg_putc = NULL;
 #endif
 
-	cpm_line_cr_cmd(pinfo, CPM_CR_STOP_TX);
-
 	if (IS_SMC(pinfo)) {
+		out_be16(&pinfo->smcup->smc_brkcr, 0);
+		cpm_line_cr_cmd(pinfo, CPM_CR_STOP_TX);
 		clrbits8(&pinfo->smcp->smc_smcm, SMCM_RX | SMCM_TX);
 		clrbits16(&pinfo->smcp->smc_smcmr, SMCMR_REN | SMCMR_TEN);
 	} else {
+		out_be16(&pinfo->sccup->scc_brkcr, 0);
+		cpm_line_cr_cmd(pinfo, CPM_CR_GRA_STOP_TX);
 		clrbits16(&pinfo->sccp->scc_sccm, UART_SCCM_TX | UART_SCCM_RX);
 		clrbits32(&pinfo->sccp->scc_gsmrl, SCC_GSMRL_ENR | SCC_GSMRL_ENT);
 	}
diff --git a/drivers/serial/dz.c b/drivers/serial/dz.c
index a81d2c2ff8a..6042b87797a 100644
--- a/drivers/serial/dz.c
+++ b/drivers/serial/dz.c
@@ -642,6 +642,26 @@ static void dz_set_termios(struct uart_port *uport, struct ktermios *termios,
 	spin_unlock_irqrestore(&dport->port.lock, flags);
 }
 
+/*
+ * Hack alert!
+ * Required solely so that the initial PROM-based console
+ * works undisturbed in parallel with this one.
+ */
+static void dz_pm(struct uart_port *uport, unsigned int state,
+		  unsigned int oldstate)
+{
+	struct dz_port *dport = to_dport(uport);
+	unsigned long flags;
+
+	spin_lock_irqsave(&dport->port.lock, flags);
+	if (state < 3)
+		dz_start_tx(&dport->port);
+	else
+		dz_stop_tx(&dport->port);
+	spin_unlock_irqrestore(&dport->port.lock, flags);
+}
+
+
 static const char *dz_type(struct uart_port *uport)
 {
 	return "DZ";
@@ -738,6 +758,7 @@ static struct uart_ops dz_ops = {
 	.startup	= dz_startup,
 	.shutdown	= dz_shutdown,
 	.set_termios	= dz_set_termios,
+	.pm		= dz_pm,
 	.type		= dz_type,
 	.release_port	= dz_release_port,
 	.request_port	= dz_request_port,
@@ -861,7 +882,10 @@ static int __init dz_console_setup(struct console *co, char *options)
 	if (ret)
 		return ret;
 
+	spin_lock_init(&dport->port.lock);	/* For dz_pm().  */
+
 	dz_reset(dport);
+	dz_pm(uport, 0, -1);
 
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
diff --git a/drivers/serial/zs.c b/drivers/serial/zs.c
index bd45b6230fd..9e6a873f820 100644
--- a/drivers/serial/zs.c
+++ b/drivers/serial/zs.c
@@ -787,7 +787,6 @@ static int zs_startup(struct uart_port *uport)
 	zport->regs[1] &= ~RxINT_MASK;
 	zport->regs[1] |= RxINT_ALL | TxINT_ENAB | EXT_INT_ENAB;
 	zport->regs[3] |= RxENABLE;
-	zport->regs[5] |= TxENAB;
 	zport->regs[15] |= BRKIE;
 	write_zsreg(zport, R1, zport->regs[1]);
 	write_zsreg(zport, R3, zport->regs[3]);
@@ -814,7 +813,6 @@ static void zs_shutdown(struct uart_port *uport)
 
 	spin_lock_irqsave(&scc->zlock, flags);
 
-	zport->regs[5] &= ~TxENAB;
 	zport->regs[3] &= ~RxENABLE;
 	write_zsreg(zport, R5, zport->regs[5]);
 	write_zsreg(zport, R3, zport->regs[3]);
@@ -959,6 +957,23 @@ static void zs_set_termios(struct uart_port *uport, struct ktermios *termios,
 	spin_unlock_irqrestore(&scc->zlock, flags);
 }
 
+/*
+ * Hack alert!
+ * Required solely so that the initial PROM-based console
+ * works undisturbed in parallel with this one.
+ */
+static void zs_pm(struct uart_port *uport, unsigned int state,
+		  unsigned int oldstate)
+{
+	struct zs_port *zport = to_zport(uport);
+
+	if (state < 3)
+		zport->regs[5] |= TxENAB;
+	else
+		zport->regs[5] &= ~TxENAB;
+	write_zsreg(zport, R5, zport->regs[5]);
+}
+
 
 static const char *zs_type(struct uart_port *uport)
 {
@@ -1041,6 +1056,7 @@ static struct uart_ops zs_ops = {
 	.startup	= zs_startup,
 	.shutdown	= zs_shutdown,
 	.set_termios	= zs_set_termios,
+	.pm		= zs_pm,
 	.type		= zs_type,
 	.release_port	= zs_release_port,
 	.request_port	= zs_request_port,
@@ -1190,6 +1206,7 @@ static int __init zs_console_setup(struct console *co, char *options)
 		return ret;
 
 	zs_reset(zport);
+	zs_pm(uport, 0, -1);
 
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 66ec5d8808d..2303521b4f0 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -49,25 +49,26 @@ config SPI_MASTER
 	  controller and the protocol drivers for the SPI slave chips
 	  that are connected.
 
+if SPI_MASTER
+
 comment "SPI Master Controller Drivers"
-	depends on SPI_MASTER
 
 config SPI_ATMEL
 	tristate "Atmel SPI Controller"
-	depends on (ARCH_AT91 || AVR32) && SPI_MASTER
+	depends on (ARCH_AT91 || AVR32)
 	help
 	  This selects a driver for the Atmel SPI Controller, present on
 	  many AT32 (AVR32) and AT91 (ARM) chips.
 
 config SPI_BFIN
 	tristate "SPI controller driver for ADI Blackfin5xx"
-	depends on SPI_MASTER && BLACKFIN
+	depends on BLACKFIN
 	help
 	  This is the SPI controller master driver for Blackfin 5xx processor.
 
 config SPI_AU1550
 	tristate "Au1550/Au12x0 SPI Controller"
-	depends on SPI_MASTER && (SOC_AU1550 || SOC_AU1200) && EXPERIMENTAL
+	depends on (SOC_AU1550 || SOC_AU1200) && EXPERIMENTAL
 	select SPI_BITBANG
 	help
 	  If you say yes to this option, support will be included for the
@@ -78,7 +79,6 @@ config SPI_AU1550
 
 config SPI_BITBANG
 	tristate "Bitbanging SPI master"
-	depends on SPI_MASTER && EXPERIMENTAL
 	help
 	  With a few GPIO pins, your system can bitbang the SPI protocol.
 	  Select this to get SPI support through I/O pins (GPIO, parallel
@@ -92,7 +92,7 @@ config SPI_BITBANG
 
 config SPI_BUTTERFLY
 	tristate "Parallel port adapter for AVR Butterfly (DEVELOPMENT)"
-	depends on SPI_MASTER && PARPORT && EXPERIMENTAL
+	depends on PARPORT
 	select SPI_BITBANG
 	help
 	  This uses a custom parallel port cable to connect to an AVR
@@ -102,14 +102,14 @@ config SPI_BUTTERFLY
 
 config SPI_IMX
 	tristate "Freescale iMX SPI controller"
-	depends on SPI_MASTER && ARCH_IMX && EXPERIMENTAL
+	depends on ARCH_IMX && EXPERIMENTAL
 	help
 	  This enables using the Freescale iMX SPI controller in master
 	  mode.
 
 config SPI_LM70_LLP
 	tristate "Parallel port adapter for LM70 eval board (DEVELOPMENT)"
-	depends on SPI_MASTER && PARPORT && EXPERIMENTAL
+	depends on PARPORT && EXPERIMENTAL
 	select SPI_BITBANG
 	help
 	  This driver supports the NS LM70 LLP Evaluation Board,
@@ -118,14 +118,14 @@ config SPI_LM70_LLP
 
 config SPI_MPC52xx_PSC
 	tristate "Freescale MPC52xx PSC SPI controller"
-	depends on SPI_MASTER && PPC_MPC52xx && EXPERIMENTAL
+	depends on PPC_MPC52xx && EXPERIMENTAL
 	help
 	  This enables using the Freescale MPC52xx Programmable Serial
 	  Controller in master SPI mode.
 
 config SPI_MPC83xx
 	tristate "Freescale MPC83xx/QUICC Engine SPI controller"
-	depends on SPI_MASTER && (PPC_83xx || QUICC_ENGINE) && EXPERIMENTAL
+	depends on (PPC_83xx || QUICC_ENGINE) && EXPERIMENTAL
 	help
 	  This enables using the Freescale MPC83xx and QUICC Engine SPI
 	  controllers in master mode.
@@ -137,21 +137,21 @@ config SPI_MPC83xx
 
 config SPI_OMAP_UWIRE
 	tristate "OMAP1 MicroWire"
-	depends on SPI_MASTER && ARCH_OMAP1
+	depends on ARCH_OMAP1
 	select SPI_BITBANG
 	help
 	  This hooks up to the MicroWire controller on OMAP1 chips.
 
 config SPI_OMAP24XX
 	tristate "McSPI driver for OMAP24xx/OMAP34xx"
-	depends on SPI_MASTER && (ARCH_OMAP24XX || ARCH_OMAP34XX)
+	depends on ARCH_OMAP24XX || ARCH_OMAP34XX
 	help
 	  SPI master controller for OMAP24xx/OMAP34xx Multichannel SPI
 	  (McSPI) modules.
 
 config SPI_PXA2XX
 	tristate "PXA2xx SSP SPI master"
-	depends on SPI_MASTER && ARCH_PXA && EXPERIMENTAL
+	depends on ARCH_PXA && EXPERIMENTAL
 	select PXA_SSP
 	help
 	  This enables using a PXA2xx SSP port as a SPI master controller.
@@ -160,14 +160,14 @@ config SPI_PXA2XX
 
 config SPI_S3C24XX
 	tristate "Samsung S3C24XX series SPI"
-	depends on SPI_MASTER && ARCH_S3C2410 && EXPERIMENTAL
+	depends on ARCH_S3C2410 && EXPERIMENTAL
 	select SPI_BITBANG
 	help
 	  SPI driver for Samsung S3C24XX series ARM SoCs
 
 config SPI_S3C24XX_GPIO
 	tristate "Samsung S3C24XX series SPI by GPIO"
-	depends on SPI_MASTER && ARCH_S3C2410 && EXPERIMENTAL
+	depends on ARCH_S3C2410 && EXPERIMENTAL
 	select SPI_BITBANG
 	help
 	  SPI driver for Samsung S3C24XX series ARM SoCs using
@@ -177,20 +177,20 @@ config SPI_S3C24XX_GPIO
 
 config SPI_SH_SCI
 	tristate "SuperH SCI SPI controller"
-	depends on SPI_MASTER && SUPERH
+	depends on SUPERH
 	select SPI_BITBANG
 	help
 	  SPI driver for SuperH SCI blocks.
 
 config SPI_TXX9
 	tristate "Toshiba TXx9 SPI controller"
-	depends on SPI_MASTER && GENERIC_GPIO && CPU_TX49XX
+	depends on GENERIC_GPIO && CPU_TX49XX
 	help
 	  SPI driver for Toshiba TXx9 MIPS SoCs
 
 config SPI_XILINX
 	tristate "Xilinx SPI controller"
-	depends on SPI_MASTER && XILINX_VIRTEX && EXPERIMENTAL
+	depends on XILINX_VIRTEX && EXPERIMENTAL
 	select SPI_BITBANG
 	help
 	  This exposes the SPI controller IP from the Xilinx EDK.
@@ -207,11 +207,10 @@ config SPI_XILINX
 # being probably the most widely used ones.
 #
 comment "SPI Protocol Masters"
-	depends on SPI_MASTER
 
 config SPI_AT25
 	tristate "SPI EEPROMs from most vendors"
-	depends on SPI_MASTER && SYSFS
+	depends on SYSFS
 	help
 	  Enable this driver to get read/write support to most SPI EEPROMs,
 	  after you configure the board init code to know about each eeprom
@@ -222,7 +221,7 @@ config SPI_AT25
 
 config SPI_SPIDEV
 	tristate "User mode SPI device driver support"
-	depends on SPI_MASTER && EXPERIMENTAL
+	depends on EXPERIMENTAL
 	help
 	  This supports user mode SPI protocol drivers.
 
@@ -231,7 +230,7 @@ config SPI_SPIDEV
 
 config SPI_TLE62X0
 	tristate "Infineon TLE62X0 (for power switching)"
-	depends on SPI_MASTER && SYSFS
+	depends on SYSFS
 	help
 	  SPI driver for Infineon TLE62X0 series line driver chips,
 	  such as the TLE6220, TLE6230 and TLE6240.  This provides a
@@ -242,6 +241,8 @@ config SPI_TLE62X0
 # Add new SPI protocol masters in alphabetical order above this line
 #
 
+endif # SPI_MASTER
+
 # (slave support would go here)
 
 endif # SPI
diff --git a/drivers/spi/atmel_spi.c b/drivers/spi/atmel_spi.c
index e81d59d7891..0c716566085 100644
--- a/drivers/spi/atmel_spi.c
+++ b/drivers/spi/atmel_spi.c
@@ -313,14 +313,14 @@ atmel_spi_dma_map_xfer(struct atmel_spi *as, struct spi_transfer *xfer)
 		xfer->tx_dma = dma_map_single(dev,
 				(void *) xfer->tx_buf, xfer->len,
 				DMA_TO_DEVICE);
-		if (dma_mapping_error(xfer->tx_dma))
+		if (dma_mapping_error(dev, xfer->tx_dma))
 			return -ENOMEM;
 	}
 	if (xfer->rx_buf) {
 		xfer->rx_dma = dma_map_single(dev,
 				xfer->rx_buf, xfer->len,
 				DMA_FROM_DEVICE);
-		if (dma_mapping_error(xfer->rx_dma)) {
+		if (dma_mapping_error(dev, xfer->rx_dma)) {
 			if (xfer->tx_buf)
 				dma_unmap_single(dev,
 						xfer->tx_dma, xfer->len,
diff --git a/drivers/spi/au1550_spi.c b/drivers/spi/au1550_spi.c
index 072c4a59533..87b73e0169c 100644
--- a/drivers/spi/au1550_spi.c
+++ b/drivers/spi/au1550_spi.c
@@ -26,6 +26,7 @@
 #include <linux/errno.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
+#include <linux/resource.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 #include <linux/dma-mapping.h>
@@ -81,6 +82,7 @@ struct au1550_spi {
 	struct spi_master *master;
 	struct device *dev;
 	struct au1550_spi_info *pdata;
+	struct resource *ioarea;
 };
 
 
@@ -96,6 +98,8 @@ static dbdev_tab_t au1550_spi_mem_dbdev =
 	.dev_intpolarity	= 0
 };
 
+static int ddma_memid;	/* id to above mem dma device */
+
 static void au1550_spi_bits_handlers_set(struct au1550_spi *hw, int bpw);
 
 
@@ -330,7 +334,7 @@ static int au1550_spi_dma_rxtmp_alloc(struct au1550_spi *hw, unsigned size)
 	hw->dma_rx_tmpbuf_size = size;
 	hw->dma_rx_tmpbuf_addr = dma_map_single(hw->dev, hw->dma_rx_tmpbuf,
 			size, DMA_FROM_DEVICE);
-	if (dma_mapping_error(hw->dma_rx_tmpbuf_addr)) {
+	if (dma_mapping_error(hw->dev, hw->dma_rx_tmpbuf_addr)) {
 		kfree(hw->dma_rx_tmpbuf);
 		hw->dma_rx_tmpbuf = 0;
 		hw->dma_rx_tmpbuf_size = 0;
@@ -374,7 +378,7 @@ static int au1550_spi_dma_txrxb(struct spi_device *spi, struct spi_transfer *t)
 			dma_rx_addr = dma_map_single(hw->dev,
 					(void *)t->rx_buf,
 					t->len, DMA_FROM_DEVICE);
-			if (dma_mapping_error(dma_rx_addr))
+			if (dma_mapping_error(hw->dev, dma_rx_addr))
 				dev_err(hw->dev, "rx dma map error\n");
 		}
 	} else {
@@ -397,7 +401,7 @@ static int au1550_spi_dma_txrxb(struct spi_device *spi, struct spi_transfer *t)
 			dma_tx_addr = dma_map_single(hw->dev,
 					(void *)t->tx_buf,
 					t->len, DMA_TO_DEVICE);
-			if (dma_mapping_error(dma_tx_addr))
+			if (dma_mapping_error(hw->dev, dma_tx_addr))
 				dev_err(hw->dev, "tx dma map error\n");
 		}
 	} else {
@@ -480,9 +484,13 @@ static irqreturn_t au1550_spi_dma_irq_callback(struct au1550_spi *hw)
 		au1xxx_dbdma_reset(hw->dma_tx_ch);
 		au1550_spi_reset_fifos(hw);
 
-		dev_err(hw->dev,
-			"Unexpected SPI error: event=0x%x stat=0x%x!\n",
-			evnt, stat);
+		if (evnt == PSC_SPIEVNT_RO)
+			dev_err(hw->dev,
+				"dma transfer: receive FIFO overflow!\n");
+		else
+			dev_err(hw->dev,
+				"dma transfer: unexpected SPI error "
+				"(event=0x%x stat=0x%x)!\n", evnt, stat);
 
 		complete(&hw->master_done);
 		return IRQ_HANDLED;
@@ -592,17 +600,17 @@ static irqreturn_t au1550_spi_pio_irq_callback(struct au1550_spi *hw)
 
 	if ((evnt & (PSC_SPIEVNT_MM | PSC_SPIEVNT_RO
 				| PSC_SPIEVNT_RU | PSC_SPIEVNT_TO
-				| PSC_SPIEVNT_TU | PSC_SPIEVNT_SD))
+				| PSC_SPIEVNT_SD))
 			!= 0) {
-		dev_err(hw->dev,
-			"Unexpected SPI error: event=0x%x stat=0x%x!\n",
-			evnt, stat);
 		/*
 		 * due to an error we consider transfer as done,
 		 * so mask all events until before next transfer start
 		 */
 		au1550_spi_mask_ack_all(hw);
 		au1550_spi_reset_fifos(hw);
+		dev_err(hw->dev,
+			"pio transfer: unexpected SPI error "
+			"(event=0x%x stat=0x%x)!\n", evnt, stat);
 		complete(&hw->master_done);
 		return IRQ_HANDLED;
 	}
@@ -616,27 +624,50 @@ static irqreturn_t au1550_spi_pio_irq_callback(struct au1550_spi *hw)
 		stat = hw->regs->psc_spistat;
 		au_sync();
 
-		if ((stat & PSC_SPISTAT_RE) == 0 && hw->rx_count < hw->len) {
+		/*
+		 * Take care to not let the Rx FIFO overflow.
+		 *
+		 * We only write a byte if we have read one at least. Initially,
+		 * the write fifo is full, so we should read from the read fifo
+		 * first.
+		 * In case we miss a word from the read fifo, we should get a
+		 * RO event and should back out.
+		 */
+		if (!(stat & PSC_SPISTAT_RE) && hw->rx_count < hw->len) {
 			hw->rx_word(hw);
-			/* ack the receive request event */
-			hw->regs->psc_spievent = PSC_SPIEVNT_RR;
-			au_sync();
 			busy = 1;
-		}
 
-		if ((stat & PSC_SPISTAT_TF) == 0 && hw->tx_count < hw->len) {
-			hw->tx_word(hw);
-			/* ack the transmit request event */
-			hw->regs->psc_spievent = PSC_SPIEVNT_TR;
-			au_sync();
-			busy = 1;
+			if (!(stat & PSC_SPISTAT_TF) && hw->tx_count < hw->len)
+				hw->tx_word(hw);
 		}
 	} while (busy);
 
-	evnt = hw->regs->psc_spievent;
+	hw->regs->psc_spievent = PSC_SPIEVNT_RR | PSC_SPIEVNT_TR;
 	au_sync();
 
-	if (hw->rx_count >= hw->len || (evnt & PSC_SPIEVNT_MD) != 0) {
+	/*
+	 * Restart the SPI transmission in case of a transmit underflow.
+	 * This seems to work despite the notes in the Au1550 data book
+	 * of Figure 8-4 with flowchart for SPI master operation:
+	 *
+	 * """Note 1: An XFR Error Interrupt occurs, unless masked,
+	 * for any of the following events: Tx FIFO Underflow,
+	 * Rx FIFO Overflow, or Multiple-master Error
+	 *    Note 2: In case of a Tx Underflow Error, all zeroes are
+	 * transmitted."""
+	 *
+	 * By simply restarting the spi transfer on Tx Underflow Error,
+	 * we assume that spi transfer was paused instead of zeroes
+	 * transmittion mentioned in the Note 2 of Au1550 data book.
+	 */
+	if (evnt & PSC_SPIEVNT_TU) {
+		hw->regs->psc_spievent = PSC_SPIEVNT_TU | PSC_SPIEVNT_MD;
+		au_sync();
+		hw->regs->psc_spipcr = PSC_SPIPCR_MS;
+		au_sync();
+	}
+
+	if (hw->rx_count >= hw->len) {
 		/* transfer completed successfully */
 		au1550_spi_mask_ack_all(hw);
 		complete(&hw->master_done);
@@ -725,6 +756,8 @@ static void __init au1550_spi_setup_psc_as_spi(struct au1550_spi *hw)
 		stat = hw->regs->psc_spistat;
 		au_sync();
 	} while ((stat & PSC_SPISTAT_DR) == 0);
+
+	au1550_spi_reset_fifos(hw);
 }
 
 
@@ -732,6 +765,7 @@ static int __init au1550_spi_probe(struct platform_device *pdev)
 {
 	struct au1550_spi *hw;
 	struct spi_master *master;
+	struct resource *r;
 	int err = 0;
 
 	master = spi_alloc_master(&pdev->dev, sizeof(struct au1550_spi));
@@ -753,76 +787,64 @@ static int __init au1550_spi_probe(struct platform_device *pdev)
 		goto err_no_pdata;
 	}
 
-	platform_set_drvdata(pdev, hw);
-
-	init_completion(&hw->master_done);
-
-	hw->bitbang.master = hw->master;
-	hw->bitbang.setup_transfer = au1550_spi_setupxfer;
-	hw->bitbang.chipselect = au1550_spi_chipsel;
-	hw->bitbang.master->setup = au1550_spi_setup;
-	hw->bitbang.txrx_bufs = au1550_spi_txrx_bufs;
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no IRQ\n");
+		err = -ENODEV;
+		goto err_no_iores;
+	}
+	hw->irq = r->start;
+
+	hw->usedma = 0;
+	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	if (r) {
+		hw->dma_tx_id = r->start;
+		r = platform_get_resource(pdev, IORESOURCE_DMA, 1);
+		if (r) {
+			hw->dma_rx_id = r->start;
+			if (usedma && ddma_memid) {
+				if (pdev->dev.dma_mask == NULL)
+					dev_warn(&pdev->dev, "no dma mask\n");
+				else
+					hw->usedma = 1;
+			}
+		}
+	}
 
-	switch (hw->pdata->bus_num) {
-	case 0:
-		hw->irq = AU1550_PSC0_INT;
-		hw->regs = (volatile psc_spi_t *)PSC0_BASE_ADDR;
-		hw->dma_rx_id = DSCR_CMD0_PSC0_RX;
-		hw->dma_tx_id = DSCR_CMD0_PSC0_TX;
-		break;
-	case 1:
-		hw->irq = AU1550_PSC1_INT;
-		hw->regs = (volatile psc_spi_t *)PSC1_BASE_ADDR;
-		hw->dma_rx_id = DSCR_CMD0_PSC1_RX;
-		hw->dma_tx_id = DSCR_CMD0_PSC1_TX;
-		break;
-	case 2:
-		hw->irq = AU1550_PSC2_INT;
-		hw->regs = (volatile psc_spi_t *)PSC2_BASE_ADDR;
-		hw->dma_rx_id = DSCR_CMD0_PSC2_RX;
-		hw->dma_tx_id = DSCR_CMD0_PSC2_TX;
-		break;
-	case 3:
-		hw->irq = AU1550_PSC3_INT;
-		hw->regs = (volatile psc_spi_t *)PSC3_BASE_ADDR;
-		hw->dma_rx_id = DSCR_CMD0_PSC3_RX;
-		hw->dma_tx_id = DSCR_CMD0_PSC3_TX;
-		break;
-	default:
-		dev_err(&pdev->dev, "Wrong bus_num of SPI\n");
-		err = -ENOENT;
-		goto err_no_pdata;
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no mmio resource\n");
+		err = -ENODEV;
+		goto err_no_iores;
 	}
 
-	if (request_mem_region((unsigned long)hw->regs, sizeof(psc_spi_t),
-			pdev->name) == NULL) {
+	hw->ioarea = request_mem_region(r->start, sizeof(psc_spi_t),
+					pdev->name);
+	if (!hw->ioarea) {
 		dev_err(&pdev->dev, "Cannot reserve iomem region\n");
 		err = -ENXIO;
 		goto err_no_iores;
 	}
 
-
-	if (usedma) {
-		if (pdev->dev.dma_mask == NULL)
-			dev_warn(&pdev->dev, "no dma mask\n");
-		else
-			hw->usedma = 1;
+	hw->regs = (psc_spi_t __iomem *)ioremap(r->start, sizeof(psc_spi_t));
+	if (!hw->regs) {
+		dev_err(&pdev->dev, "cannot ioremap\n");
+		err = -ENXIO;
+		goto err_ioremap;
 	}
 
-	if (hw->usedma) {
-		/*
-		 * create memory device with 8 bits dev_devwidth
-		 * needed for proper byte ordering to spi fifo
-		 */
-		int memid = au1xxx_ddma_add_device(&au1550_spi_mem_dbdev);
-		if (!memid) {
-			dev_err(&pdev->dev,
-				"Cannot create dma 8 bit mem device\n");
-			err = -ENXIO;
-			goto err_dma_add_dev;
-		}
+	platform_set_drvdata(pdev, hw);
+
+	init_completion(&hw->master_done);
+
+	hw->bitbang.master = hw->master;
+	hw->bitbang.setup_transfer = au1550_spi_setupxfer;
+	hw->bitbang.chipselect = au1550_spi_chipsel;
+	hw->bitbang.master->setup = au1550_spi_setup;
+	hw->bitbang.txrx_bufs = au1550_spi_txrx_bufs;
 
-		hw->dma_tx_ch = au1xxx_dbdma_chan_alloc(memid,
+	if (hw->usedma) {
+		hw->dma_tx_ch = au1xxx_dbdma_chan_alloc(ddma_memid,
 			hw->dma_tx_id, NULL, (void *)hw);
 		if (hw->dma_tx_ch == 0) {
 			dev_err(&pdev->dev,
@@ -841,7 +863,7 @@ static int __init au1550_spi_probe(struct platform_device *pdev)
 
 
 		hw->dma_rx_ch = au1xxx_dbdma_chan_alloc(hw->dma_rx_id,
-			memid, NULL, (void *)hw);
+			ddma_memid, NULL, (void *)hw);
 		if (hw->dma_rx_ch == 0) {
 			dev_err(&pdev->dev,
 				"Cannot allocate rx dma channel\n");
@@ -874,7 +896,7 @@ static int __init au1550_spi_probe(struct platform_device *pdev)
 		goto err_no_irq;
 	}
 
-	master->bus_num = hw->pdata->bus_num;
+	master->bus_num = pdev->id;
 	master->num_chipselect = hw->pdata->num_chipselect;
 
 	/*
@@ -924,8 +946,11 @@ err_no_txdma_descr:
 		au1xxx_dbdma_chan_free(hw->dma_tx_ch);
 
 err_no_txdma:
-err_dma_add_dev:
-	release_mem_region((unsigned long)hw->regs, sizeof(psc_spi_t));
+	iounmap((void __iomem *)hw->regs);
+
+err_ioremap:
+	release_resource(hw->ioarea);
+	kfree(hw->ioarea);
 
 err_no_iores:
 err_no_pdata:
@@ -944,7 +969,9 @@ static int __exit au1550_spi_remove(struct platform_device *pdev)
 
 	spi_bitbang_stop(&hw->bitbang);
 	free_irq(hw->irq, hw);
-	release_mem_region((unsigned long)hw->regs, sizeof(psc_spi_t));
+	iounmap((void __iomem *)hw->regs);
+	release_resource(hw->ioarea);
+	kfree(hw->ioarea);
 
 	if (hw->usedma) {
 		au1550_spi_dma_rxtmp_free(hw);
@@ -971,12 +998,24 @@ static struct platform_driver au1550_spi_drv = {
 
 static int __init au1550_spi_init(void)
 {
+	/*
+	 * create memory device with 8 bits dev_devwidth
+	 * needed for proper byte ordering to spi fifo
+	 */
+	if (usedma) {
+		ddma_memid = au1xxx_ddma_add_device(&au1550_spi_mem_dbdev);
+		if (!ddma_memid)
+			printk(KERN_ERR "au1550-spi: cannot add memory"
+					"dbdma device\n");
+	}
 	return platform_driver_probe(&au1550_spi_drv, au1550_spi_probe);
 }
 module_init(au1550_spi_init);
 
 static void __exit au1550_spi_exit(void)
 {
+	if (usedma && ddma_memid)
+		au1xxx_ddma_del_device(ddma_memid);
 	platform_driver_unregister(&au1550_spi_drv);
 }
 module_exit(au1550_spi_exit);
diff --git a/drivers/spi/omap2_mcspi.c b/drivers/spi/omap2_mcspi.c
index b1cc148036c..f6f987bb71c 100644
--- a/drivers/spi/omap2_mcspi.c
+++ b/drivers/spi/omap2_mcspi.c
@@ -836,7 +836,7 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 		if (tx_buf != NULL) {
 			t->tx_dma = dma_map_single(&spi->dev, (void *) tx_buf,
 					len, DMA_TO_DEVICE);
-			if (dma_mapping_error(t->tx_dma)) {
+			if (dma_mapping_error(&spi->dev, t->tx_dma)) {
 				dev_dbg(&spi->dev, "dma %cX %d bytes error\n",
 						'T', len);
 				return -EINVAL;
@@ -845,7 +845,7 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 		if (rx_buf != NULL) {
 			t->rx_dma = dma_map_single(&spi->dev, rx_buf, t->len,
 					DMA_FROM_DEVICE);
-			if (dma_mapping_error(t->rx_dma)) {
+			if (dma_mapping_error(&spi->dev, t->rx_dma)) {
 				dev_dbg(&spi->dev, "dma %cX %d bytes error\n",
 						'R', len);
 				if (tx_buf != NULL)
diff --git a/drivers/spi/pxa2xx_spi.c b/drivers/spi/pxa2xx_spi.c
index 0c452c46ab0..067299d6d19 100644
--- a/drivers/spi/pxa2xx_spi.c
+++ b/drivers/spi/pxa2xx_spi.c
@@ -353,7 +353,7 @@ static int map_dma_buffers(struct driver_data *drv_data)
 	drv_data->rx_dma = dma_map_single(dev, drv_data->rx,
 						drv_data->rx_map_len,
 						DMA_FROM_DEVICE);
-	if (dma_mapping_error(drv_data->rx_dma))
+	if (dma_mapping_error(dev, drv_data->rx_dma))
 		return 0;
 
 	/* Stream map the tx buffer */
@@ -361,7 +361,7 @@ static int map_dma_buffers(struct driver_data *drv_data)
 						drv_data->tx_map_len,
 						DMA_TO_DEVICE);
 
-	if (dma_mapping_error(drv_data->tx_dma)) {
+	if (dma_mapping_error(dev, drv_data->tx_dma)) {
 		dma_unmap_single(dev, drv_data->rx_dma,
 					drv_data->rx_map_len, DMA_FROM_DEVICE);
 		return 0;
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 1771b2456bf..ecca4a6a6f9 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -218,6 +218,8 @@ struct spi_device *spi_new_device(struct spi_master *master,
 	if (!spi_master_get(master))
 		return NULL;
 
+	WARN_ON(strlen(chip->modalias) >= sizeof(proxy->modalias));
+
 	proxy = kzalloc(sizeof *proxy, GFP_KERNEL);
 	if (!proxy) {
 		dev_err(dev, "can't alloc dev for cs%d\n",
@@ -229,7 +231,7 @@ struct spi_device *spi_new_device(struct spi_master *master,
 	proxy->max_speed_hz = chip->max_speed_hz;
 	proxy->mode = chip->mode;
 	proxy->irq = chip->irq;
-	proxy->modalias = chip->modalias;
+	strlcpy(proxy->modalias, chip->modalias, sizeof(proxy->modalias));
 
 	snprintf(proxy->dev.bus_id, sizeof proxy->dev.bus_id,
 			"%s.%u", master->dev.bus_id,
diff --git a/drivers/spi/spi_imx.c b/drivers/spi/spi_imx.c
index 54ac7bea5f8..6fb77fcc497 100644
--- a/drivers/spi/spi_imx.c
+++ b/drivers/spi/spi_imx.c
@@ -491,7 +491,7 @@ static int map_dma_buffers(struct driver_data *drv_data)
 							buf,
 							drv_data->tx_map_len,
 							DMA_TO_DEVICE);
-			if (dma_mapping_error(drv_data->tx_dma))
+			if (dma_mapping_error(dev, drv_data->tx_dma))
 				return -1;
 
 			drv_data->tx_dma_needs_unmap = 1;
@@ -516,7 +516,7 @@ static int map_dma_buffers(struct driver_data *drv_data)
 					buf,
 					drv_data->len,
 					DMA_FROM_DEVICE);
-		if (dma_mapping_error(drv_data->rx_dma))
+		if (dma_mapping_error(dev, drv_data->rx_dma))
 			return -1;
 		drv_data->rx_dma_needs_unmap = 1;
 	}
@@ -534,7 +534,7 @@ static int map_dma_buffers(struct driver_data *drv_data)
 					buf,
 					drv_data->tx_map_len,
 					DMA_TO_DEVICE);
-	if (dma_mapping_error(drv_data->tx_dma)) {
+	if (dma_mapping_error(dev, drv_data->tx_dma)) {
 		if (drv_data->rx_dma) {
 			dma_unmap_single(dev,
 					drv_data->rx_dma,
diff --git a/drivers/spi/spi_mpc83xx.c b/drivers/spi/spi_mpc83xx.c
index 6832da6f710..070c6219e2d 100644
--- a/drivers/spi/spi_mpc83xx.c
+++ b/drivers/spi/spi_mpc83xx.c
@@ -266,21 +266,24 @@ int mpc83xx_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 
 	cs->hw_mode |= SPMODE_LEN(bits_per_word);
 
-	if ((mpc83xx_spi->spibrg / hz) >= 64) {
-		pm = mpc83xx_spi->spibrg / (hz * 64) - 1;
-		if (pm > 0x0f) {
-			dev_err(&spi->dev, "Requested speed is too "
-				"low: %d Hz. Will use %d Hz instead.\n",
-				hz, mpc83xx_spi->spibrg / 1024);
-			pm = 0x0f;
+	if ((mpc83xx_spi->spibrg / hz) > 64) {
+		pm = mpc83xx_spi->spibrg / (hz * 64);
+		if (pm > 16) {
+			cs->hw_mode |= SPMODE_DIV16;
+			pm /= 16;
+			if (pm > 16) {
+				dev_err(&spi->dev, "Requested speed is too "
+					"low: %d Hz. Will use %d Hz instead.\n",
+					hz, mpc83xx_spi->spibrg / 1024);
+				pm = 16;
+			}
 		}
-		cs->hw_mode |= SPMODE_PM(pm) | SPMODE_DIV16;
-	} else {
+	} else
 		pm = mpc83xx_spi->spibrg / (hz * 4);
-		if (pm)
-			pm--;
-		cs->hw_mode |= SPMODE_PM(pm);
-	}
+	if (pm)
+		pm--;
+
+	cs->hw_mode |= SPMODE_PM(pm);
 	regval =  mpc83xx_spi_read_reg(&mpc83xx_spi->base->mode);
 	if (cs->hw_mode != regval) {
 		unsigned long flags;
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 2833fd772a2..e5e0cfed5e3 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -228,7 +228,6 @@ static int spidev_message(struct spidev_data *spidev,
 	 * We walk the array of user-provided transfers, using each one
 	 * to initialize a kernel version of the same transfer.
 	 */
-	mutex_lock(&spidev->buf_lock);
 	buf = spidev->buffer;
 	total = 0;
 	for (n = n_xfers, k_tmp = k_xfers, u_tmp = u_xfers;
@@ -296,14 +295,12 @@ static int spidev_message(struct spidev_data *spidev,
 	status = total;
 
 done:
-	mutex_unlock(&spidev->buf_lock);
 	kfree(k_xfers);
 	return status;
 }
 
-static int
-spidev_ioctl(struct inode *inode, struct file *filp,
-		unsigned int cmd, unsigned long arg)
+static long
+spidev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
 	int			err = 0;
 	int			retval = 0;
@@ -341,6 +338,14 @@ spidev_ioctl(struct inode *inode, struct file *filp,
 	if (spi == NULL)
 		return -ESHUTDOWN;
 
+	/* use the buffer lock here for triple duty:
+	 *  - prevent I/O (from us) so calling spi_setup() is safe;
+	 *  - prevent concurrent SPI_IOC_WR_* from morphing
+	 *    data fields while SPI_IOC_RD_* reads them;
+	 *  - SPI_IOC_MESSAGE needs the buffer locked "normally".
+	 */
+	mutex_lock(&spidev->buf_lock);
+
 	switch (cmd) {
 	/* read requests */
 	case SPI_IOC_RD_MODE:
@@ -456,6 +461,8 @@ spidev_ioctl(struct inode *inode, struct file *filp,
 		kfree(ioc);
 		break;
 	}
+
+	mutex_unlock(&spidev->buf_lock);
 	spi_dev_put(spi);
 	return retval;
 }
@@ -533,7 +540,7 @@ static struct file_operations spidev_fops = {
 	 */
 	.write =	spidev_write,
 	.read =		spidev_read,
-	.ioctl =	spidev_ioctl,
+	.unlocked_ioctl = spidev_ioctl,
 	.open =		spidev_open,
 	.release =	spidev_release,
 };
diff --git a/drivers/spi/xilinx_spi.c b/drivers/spi/xilinx_spi.c
index 113a0468ffc..68d6f4988fb 100644
--- a/drivers/spi/xilinx_spi.c
+++ b/drivers/spi/xilinx_spi.c
@@ -353,11 +353,12 @@ static int __init xilinx_spi_probe(struct platform_device *dev)
 		goto put_master;
 	}
 
-	xspi->irq = platform_get_irq(dev, 0);
-	if (xspi->irq < 0) {
+	ret = platform_get_irq(dev, 0);
+	if (ret < 0) {
 		ret = -ENXIO;
 		goto unmap_io;
 	}
+	xspi->irq = ret;
 
 	master->bus_num = pdata->bus_num;
 	master->num_chipselect = pdata->num_chipselect;
diff --git a/drivers/telephony/ixj.c b/drivers/telephony/ixj.c
index 49cd9793404..ec7aeb502d1 100644
--- a/drivers/telephony/ixj.c
+++ b/drivers/telephony/ixj.c
@@ -6095,15 +6095,15 @@ static int capabilities_check(IXJ *j, struct phone_capability *pcreq)
 	return retval;
 }
 
-static int ixj_ioctl(struct inode *inode, struct file *file_p, unsigned int cmd, unsigned long arg)
+static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long arg)
 {
 	IXJ_TONE ti;
 	IXJ_FILTER jf;
 	IXJ_FILTER_RAW jfr;
 	void __user *argp = (void __user *)arg;
-
-	unsigned int raise, mant;
+	struct inode *inode = file_p->f_path.dentry->d_inode;
 	unsigned int minor = iminor(inode);
+	unsigned int raise, mant;
 	int board = NUM(inode);
 
 	IXJ *j = get_ixj(NUM(inode));
@@ -6661,6 +6661,15 @@ static int ixj_ioctl(struct inode *inode, struct file *file_p, unsigned int cmd,
 	return retval;
 }
 
+static long ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long arg)
+{
+	long ret;
+	lock_kernel();
+	ret = do_ixj_ioctl(file_p, cmd, arg);
+	unlock_kernel();
+	return ret;
+}
+
 static int ixj_fasync(int fd, struct file *file_p, int mode)
 {
 	IXJ *j = get_ixj(NUM(file_p->f_path.dentry->d_inode));
@@ -6674,7 +6683,7 @@ static const struct file_operations ixj_fops =
         .read           = ixj_enhanced_read,
         .write          = ixj_enhanced_write,
         .poll           = ixj_poll,
-        .ioctl          = ixj_ioctl,
+        .unlocked_ioctl = ixj_ioctl,
         .release        = ixj_release,
         .fasync         = ixj_fasync
 };
diff --git a/drivers/usb/gadget/at91_udc.h b/drivers/usb/gadget/at91_udc.h
index a973f2a50fb..c65d6229589 100644
--- a/drivers/usb/gadget/at91_udc.h
+++ b/drivers/usb/gadget/at91_udc.h
@@ -171,7 +171,7 @@ struct at91_request {
 #endif
 
 #define ERR(stuff...)		pr_err("udc: " stuff)
-#define WARN(stuff...)		pr_warning("udc: " stuff)
+#define WARNING(stuff...)	pr_warning("udc: " stuff)
 #define INFO(stuff...)		pr_info("udc: " stuff)
 #define DBG(stuff...)		pr_debug("udc: " stuff)
 
diff --git a/drivers/usb/gadget/cdc2.c b/drivers/usb/gadget/cdc2.c
index d490d028950..a39a4b940c3 100644
--- a/drivers/usb/gadget/cdc2.c
+++ b/drivers/usb/gadget/cdc2.c
@@ -170,7 +170,7 @@ static int __init cdc_bind(struct usb_composite_dev *cdev)
 		 * but if the controller isn't recognized at all then
 		 * that assumption is a bit more likely to be wrong.
 		 */
-		WARN(cdev, "controller '%s' not recognized; trying %s\n",
+		WARNING(cdev, "controller '%s' not recognized; trying %s\n",
 				gadget->name,
 				cdc_config_driver.label);
 		device_desc.bcdDevice =
diff --git a/drivers/usb/gadget/ether.c b/drivers/usb/gadget/ether.c
index d7aaaa29b1e..bcac2e68660 100644
--- a/drivers/usb/gadget/ether.c
+++ b/drivers/usb/gadget/ether.c
@@ -293,7 +293,7 @@ static int __init eth_bind(struct usb_composite_dev *cdev)
 		 * but if the controller isn't recognized at all then
 		 * that assumption is a bit more likely to be wrong.
 		 */
-		WARN(cdev, "controller '%s' not recognized; trying %s\n",
+		WARNING(cdev, "controller '%s' not recognized; trying %s\n",
 				gadget->name,
 				eth_config_driver.label);
 		device_desc.bcdDevice =
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 15c24edbb61..ea2c31d1808 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -308,7 +308,7 @@ MODULE_LICENSE("Dual BSD/GPL");
 	dev_vdbg(&(d)->gadget->dev , fmt , ## args)
 #define ERROR(d, fmt, args...) \
 	dev_err(&(d)->gadget->dev , fmt , ## args)
-#define WARN(d, fmt, args...) \
+#define WARNING(d, fmt, args...) \
 	dev_warn(&(d)->gadget->dev , fmt , ## args)
 #define INFO(d, fmt, args...) \
 	dev_info(&(d)->gadget->dev , fmt , ## args)
@@ -1091,7 +1091,7 @@ static int ep0_queue(struct fsg_dev *fsg)
 	if (rc != 0 && rc != -ESHUTDOWN) {
 
 		/* We can't do much more than wait for a reset */
-		WARN(fsg, "error in submission: %s --> %d\n",
+		WARNING(fsg, "error in submission: %s --> %d\n",
 				fsg->ep0->name, rc);
 	}
 	return rc;
@@ -1227,7 +1227,7 @@ static void received_cbi_adsc(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 
 	/* Save the command for later */
 	if (fsg->cbbuf_cmnd_size)
-		WARN(fsg, "CB[I] overwriting previous command\n");
+		WARNING(fsg, "CB[I] overwriting previous command\n");
 	fsg->cbbuf_cmnd_size = req->actual;
 	memcpy(fsg->cbbuf_cmnd, req->buf, fsg->cbbuf_cmnd_size);
 
@@ -1506,7 +1506,7 @@ static void start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
 		 * submissions if DMA is enabled. */
 		if (rc != -ESHUTDOWN && !(rc == -EOPNOTSUPP &&
 						req->length == 0))
-			WARN(fsg, "error in submission: %s --> %d\n",
+			WARNING(fsg, "error in submission: %s --> %d\n",
 					ep->name, rc);
 	}
 }
@@ -2294,7 +2294,7 @@ static int halt_bulk_in_endpoint(struct fsg_dev *fsg)
 		VDBG(fsg, "delayed bulk-in endpoint halt\n");
 	while (rc != 0) {
 		if (rc != -EAGAIN) {
-			WARN(fsg, "usb_ep_set_halt -> %d\n", rc);
+			WARNING(fsg, "usb_ep_set_halt -> %d\n", rc);
 			rc = 0;
 			break;
 		}
@@ -2317,7 +2317,7 @@ static int wedge_bulk_in_endpoint(struct fsg_dev *fsg)
 		VDBG(fsg, "delayed bulk-in endpoint wedge\n");
 	while (rc != 0) {
 		if (rc != -EAGAIN) {
-			WARN(fsg, "usb_ep_set_wedge -> %d\n", rc);
+			WARNING(fsg, "usb_ep_set_wedge -> %d\n", rc);
 			rc = 0;
 			break;
 		}
@@ -3755,7 +3755,7 @@ static int __init check_parameters(struct fsg_dev *fsg)
 		if (gcnum >= 0)
 			mod_data.release = 0x0300 + gcnum;
 		else {
-			WARN(fsg, "controller '%s' not recognized\n",
+			WARNING(fsg, "controller '%s' not recognized\n",
 				fsg->gadget->name);
 			mod_data.release = 0x0399;
 		}
diff --git a/drivers/usb/gadget/fsl_usb2_udc.c b/drivers/usb/gadget/fsl_usb2_udc.c
index 1695382f30f..1cfccf102a2 100644
--- a/drivers/usb/gadget/fsl_usb2_udc.c
+++ b/drivers/usb/gadget/fsl_usb2_udc.c
@@ -1538,7 +1538,7 @@ static void dtd_complete_irq(struct fsl_udc *udc)
 
 		/* If the ep is configured */
 		if (curr_ep->name == NULL) {
-			WARN("Invalid EP?");
+			WARNING("Invalid EP?");
 			continue;
 		}
 
diff --git a/drivers/usb/gadget/fsl_usb2_udc.h b/drivers/usb/gadget/fsl_usb2_udc.h
index 98b1483ef6a..6131752a38b 100644
--- a/drivers/usb/gadget/fsl_usb2_udc.h
+++ b/drivers/usb/gadget/fsl_usb2_udc.h
@@ -552,7 +552,7 @@ static void dump_msg(const char *label, const u8 * buf, unsigned int length)
 #endif
 
 #define ERR(stuff...)		pr_err("udc: " stuff)
-#define WARN(stuff...)		pr_warning("udc: " stuff)
+#define WARNING(stuff...)		pr_warning("udc: " stuff)
 #define INFO(stuff...)		pr_info("udc: " stuff)
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/gadget/gmidi.c b/drivers/usb/gadget/gmidi.c
index 7f4d4828e3a..ea8651e3da1 100644
--- a/drivers/usb/gadget/gmidi.c
+++ b/drivers/usb/gadget/gmidi.c
@@ -138,8 +138,6 @@ static void gmidi_transmit(struct gmidi_device* dev, struct usb_request* req);
 	dev_vdbg(&(d)->gadget->dev , fmt , ## args)
 #define ERROR(d, fmt, args...) \
 	dev_err(&(d)->gadget->dev , fmt , ## args)
-#define WARN(d, fmt, args...) \
-	dev_warn(&(d)->gadget->dev , fmt , ## args)
 #define INFO(d, fmt, args...) \
 	dev_info(&(d)->gadget->dev , fmt , ## args)
 
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 48f1c63b701..60aa04847b1 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -1768,7 +1768,7 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	 * usb_gadget_driver_{register,unregister}() must change.
 	 */
 	if (the_controller) {
-		WARN(dev, "ignoring %s\n", pci_name(pdev));
+		WARNING(dev, "ignoring %s\n", pci_name(pdev));
 		return -EBUSY;
 	}
 	if (!pdev->irq) {
diff --git a/drivers/usb/gadget/goku_udc.h b/drivers/usb/gadget/goku_udc.h
index bc4eb1e0b50..566cb231905 100644
--- a/drivers/usb/gadget/goku_udc.h
+++ b/drivers/usb/gadget/goku_udc.h
@@ -285,7 +285,7 @@ struct goku_udc {
 
 #define ERROR(dev,fmt,args...) \
 	xprintk(dev , KERN_ERR , fmt , ## args)
-#define WARN(dev,fmt,args...) \
+#define WARNING(dev,fmt,args...) \
 	xprintk(dev , KERN_WARNING , fmt , ## args)
 #define INFO(dev,fmt,args...) \
 	xprintk(dev , KERN_INFO , fmt , ## args)
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index 04692d59fc1..f4585d3e90d 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -262,8 +262,6 @@ static const char *CHIP;
 
 #define ERROR(dev,fmt,args...) \
 	xprintk(dev , KERN_ERR , fmt , ## args)
-#define WARN(dev,fmt,args...) \
-	xprintk(dev , KERN_WARNING , fmt , ## args)
 #define INFO(dev,fmt,args...) \
 	xprintk(dev , KERN_INFO , fmt , ## args)
 
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index b67ab677af7..5cfb5ebf388 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -1007,7 +1007,7 @@ static void scan_dma_completions (struct net2280_ep *ep)
 			 * 0122, and 0124; not all cases trigger the warning.
 			 */
 			if ((tmp & (1 << NAK_OUT_PACKETS)) == 0) {
-				WARN (ep->dev, "%s lost packet sync!\n",
+				WARNING (ep->dev, "%s lost packet sync!\n",
 						ep->ep.name);
 				req->req.status = -EOVERFLOW;
 			} else if ((tmp = readl (&ep->regs->ep_avail)) != 0) {
diff --git a/drivers/usb/gadget/net2280.h b/drivers/usb/gadget/net2280.h
index 1f2af398a9a..81a71dbdc2c 100644
--- a/drivers/usb/gadget/net2280.h
+++ b/drivers/usb/gadget/net2280.h
@@ -272,7 +272,7 @@ static inline void net2280_led_shutdown (struct net2280 *dev)
 
 #define ERROR(dev,fmt,args...) \
 	xprintk(dev , KERN_ERR , fmt , ## args)
-#define WARN(dev,fmt,args...) \
+#define WARNING(dev,fmt,args...) \
 	xprintk(dev , KERN_WARNING , fmt , ## args)
 #define INFO(dev,fmt,args...) \
 	xprintk(dev , KERN_INFO , fmt , ## args)
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index 4b79a8509e8..395bd184448 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -1120,7 +1120,7 @@ static int omap_ep_set_halt(struct usb_ep *_ep, int value)
 			status = -EINVAL;
 		else if (value) {
 			if (ep->udc->ep0_set_config) {
-				WARN("error changing config?\n");
+				WARNING("error changing config?\n");
 				omap_writew(UDC_CLR_CFG, UDC_SYSCON2);
 			}
 			omap_writew(UDC_STALL_CMD, UDC_SYSCON2);
@@ -1764,7 +1764,7 @@ do_stall:
 					u.r.bRequestType, u.r.bRequest, status);
 			if (udc->ep0_set_config) {
 				if (udc->ep0_reset_config)
-					WARN("error resetting config?\n");
+					WARNING("error resetting config?\n");
 				else
 					omap_writew(UDC_CLR_CFG, UDC_SYSCON2);
 			}
@@ -3076,7 +3076,7 @@ static int omap_udc_suspend(struct platform_device *dev, pm_message_t message)
 	 * which would prevent entry to deep sleep...
 	 */
 	if ((devstat & UDC_ATT) != 0 && (devstat & UDC_SUS) == 0) {
-		WARN("session active; suspend requires disconnect\n");
+		WARNING("session active; suspend requires disconnect\n");
 		omap_pullup(&udc->gadget, 0);
 	}
 
diff --git a/drivers/usb/gadget/omap_udc.h b/drivers/usb/gadget/omap_udc.h
index 8522bbb1227..29edc51b6b2 100644
--- a/drivers/usb/gadget/omap_udc.h
+++ b/drivers/usb/gadget/omap_udc.h
@@ -188,7 +188,7 @@ struct omap_udc {
 #endif
 
 #define ERR(stuff...)		pr_err("udc: " stuff)
-#define WARN(stuff...)		pr_warning("udc: " stuff)
+#define WARNING(stuff...)	pr_warning("udc: " stuff)
 #define INFO(stuff...)		pr_info("udc: " stuff)
 #define DBG(stuff...)		pr_debug("udc: " stuff)
 
diff --git a/drivers/usb/gadget/printer.c b/drivers/usb/gadget/printer.c
index 49cd9e145a9..e0090085b78 100644
--- a/drivers/usb/gadget/printer.c
+++ b/drivers/usb/gadget/printer.c
@@ -179,7 +179,7 @@ module_param(qlen, uint, S_IRUGO|S_IWUSR);
 
 #define ERROR(dev, fmt, args...) \
 	xprintk(dev, KERN_ERR, fmt, ## args)
-#define WARN(dev, fmt, args...) \
+#define WARNING(dev, fmt, args...) \
 	xprintk(dev, KERN_WARNING, fmt, ## args)
 #define INFO(dev, fmt, args...) \
 	xprintk(dev, KERN_INFO, fmt, ## args)
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index 8fb0066609b..7e6725d8997 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -342,7 +342,7 @@ pxa25x_ep_free_request (struct usb_ep *_ep, struct usb_request *_req)
 	struct pxa25x_request	*req;
 
 	req = container_of (_req, struct pxa25x_request, req);
-	WARN_ON (!list_empty (&req->queue));
+	WARN_ON(!list_empty (&req->queue));
 	kfree(req);
 }
 
@@ -1556,7 +1556,7 @@ config_change:
 					 * tell us about config change events,
 					 * so later ones may fail...
 					 */
-					WARN("config change %02x fail %d?\n",
+					WARNING("config change %02x fail %d?\n",
 						u.r.bRequest, i);
 					return;
 					/* TODO experiment:  if has_cfr,
@@ -2330,7 +2330,7 @@ static int pxa25x_udc_suspend(struct platform_device *dev, pm_message_t state)
 	unsigned long flags;
 
 	if (!udc->mach->gpio_pullup && !udc->mach->udc_command)
-		WARN("USB host won't detect disconnect!\n");
+		WARNING("USB host won't detect disconnect!\n");
 	udc->suspended = 1;
 
 	local_irq_save(flags);
diff --git a/drivers/usb/gadget/pxa25x_udc.h b/drivers/usb/gadget/pxa25x_udc.h
index 4d11ece7c95..c8a13215e02 100644
--- a/drivers/usb/gadget/pxa25x_udc.h
+++ b/drivers/usb/gadget/pxa25x_udc.h
@@ -259,7 +259,7 @@ dump_state(struct pxa25x_udc *dev)
 #define DBG(lvl, stuff...) do{if ((lvl) <= UDC_DEBUG) DMSG(stuff);}while(0)
 
 #define ERR(stuff...)		pr_err("udc: " stuff)
-#define WARN(stuff...)		pr_warning("udc: " stuff)
+#define WARNING(stuff...)	pr_warning("udc: " stuff)
 #define INFO(stuff...)		pr_info("udc: " stuff)
 
 
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 5458f43a866..3791e627190 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -116,7 +116,6 @@ static inline int qlen(struct usb_gadget *gadget)
 #undef DBG
 #undef VDBG
 #undef ERROR
-#undef WARN
 #undef INFO
 
 #define xprintk(d, level, fmt, args...) \
@@ -140,8 +139,6 @@ static inline int qlen(struct usb_gadget *gadget)
 
 #define ERROR(dev, fmt, args...) \
 	xprintk(dev , KERN_ERR , fmt , ## args)
-#define WARN(dev, fmt, args...) \
-	xprintk(dev , KERN_WARNING , fmt , ## args)
 #define INFO(dev, fmt, args...) \
 	xprintk(dev , KERN_INFO , fmt , ## args)
 
diff --git a/drivers/usb/host/isp116x-hcd.c b/drivers/usb/host/isp116x-hcd.c
index 31178e10cbb..ce1ca0ba051 100644
--- a/drivers/usb/host/isp116x-hcd.c
+++ b/drivers/usb/host/isp116x-hcd.c
@@ -882,7 +882,7 @@ static void isp116x_endpoint_disable(struct usb_hcd *hcd,
 	for (i = 0; i < 100 && !list_empty(&hep->urb_list); i++)
 		msleep(3);
 	if (!list_empty(&hep->urb_list))
-		WARN("ep %p not empty?\n", ep);
+		WARNING("ep %p not empty?\n", ep);
 
 	kfree(ep);
 	hep->hcpriv = NULL;
diff --git a/drivers/usb/host/isp116x.h b/drivers/usb/host/isp116x.h
index 595b90a9984..aa211bafcff 100644
--- a/drivers/usb/host/isp116x.h
+++ b/drivers/usb/host/isp116x.h
@@ -338,7 +338,7 @@ struct isp116x_ep {
 #endif
 
 #define ERR(stuff...)		printk(KERN_ERR "116x: " stuff)
-#define WARN(stuff...)		printk(KERN_WARNING "116x: " stuff)
+#define WARNING(stuff...)	printk(KERN_WARNING "116x: " stuff)
 #define INFO(stuff...)		printk(KERN_INFO "116x: " stuff)
 
 /* ------------------------------------------------- */
diff --git a/drivers/usb/host/sl811-hcd.c b/drivers/usb/host/sl811-hcd.c
index 340d72da554..8a74bbb57d0 100644
--- a/drivers/usb/host/sl811-hcd.c
+++ b/drivers/usb/host/sl811-hcd.c
@@ -1026,7 +1026,7 @@ sl811h_endpoint_disable(struct usb_hcd *hcd, struct usb_host_endpoint *hep)
 	if (!list_empty(&hep->urb_list))
 		msleep(3);
 	if (!list_empty(&hep->urb_list))
-		WARN("ep %p not empty?\n", ep);
+		WARNING("ep %p not empty?\n", ep);
 
 	kfree(ep);
 	hep->hcpriv = NULL;
diff --git a/drivers/usb/host/sl811.h b/drivers/usb/host/sl811.h
index 7690d98e42a..b6b8c1f233d 100644
--- a/drivers/usb/host/sl811.h
+++ b/drivers/usb/host/sl811.h
@@ -261,6 +261,6 @@ sl811_read_buf(struct sl811 *sl811, int addr, void *buf, size_t count)
 #endif
 
 #define ERR(stuff...)		printk(KERN_ERR "sl811: " stuff)
-#define WARN(stuff...)		printk(KERN_WARNING "sl811: " stuff)
+#define WARNING(stuff...)	printk(KERN_WARNING "sl811: " stuff)
 #define INFO(stuff...)		printk(KERN_INFO "sl811: " stuff)
 
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index 054dedd2812..b358c4e1cf2 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -81,7 +81,7 @@ static struct usb_device *testdev_to_usbdev (struct usbtest_dev *test)
 
 #define ERROR(tdev, fmt, args...) \
 	dev_err(&(tdev)->intf->dev , fmt , ## args)
-#define WARN(tdev, fmt, args...) \
+#define WARNING(tdev, fmt, args...) \
 	dev_warn(&(tdev)->intf->dev , fmt , ## args)
 
 /*-------------------------------------------------------------------------*/
@@ -1946,7 +1946,7 @@ usbtest_probe (struct usb_interface *intf, const struct usb_device_id *id)
 
 			status = get_endpoints (dev, intf);
 			if (status < 0) {
-				WARN(dev, "couldn't get endpoints, %d\n",
+				WARNING(dev, "couldn't get endpoints, %d\n",
 						status);
 				return status;
 			}
diff --git a/drivers/usb/mon/mon_text.c b/drivers/usb/mon/mon_text.c
index 5e3e4e9b6c7..1f715436d6d 100644
--- a/drivers/usb/mon/mon_text.c
+++ b/drivers/usb/mon/mon_text.c
@@ -87,7 +87,7 @@ struct mon_reader_text {
 
 static struct dentry *mon_dir;		/* Usually /sys/kernel/debug/usbmon */
 
-static void mon_text_ctor(struct kmem_cache *, void *);
+static void mon_text_ctor(void *);
 
 struct mon_text_ptr {
 	int cnt, limit;
@@ -720,7 +720,7 @@ void mon_text_del(struct mon_bus *mbus)
 /*
  * Slab interface: constructor.
  */
-static void mon_text_ctor(struct kmem_cache *slab, void *mem)
+static void mon_text_ctor(void *mem)
 {
 	/*
 	 * Nothing to initialize. No, really!
diff --git a/drivers/usb/serial/ipaq.c b/drivers/usb/serial/ipaq.c
index 832a5a4f3cb..cd9a2e138c8 100644
--- a/drivers/usb/serial/ipaq.c
+++ b/drivers/usb/serial/ipaq.c
@@ -651,15 +651,17 @@ static int ipaq_open(struct tty_struct *tty,
 	 */
 
 	kfree(port->bulk_in_buffer);
+	kfree(port->bulk_out_buffer);
+	/* make sure the generic serial code knows */
+	port->bulk_out_buffer = NULL;
+
 	port->bulk_in_buffer = kmalloc(URBDATA_SIZE, GFP_KERNEL);
-	if (port->bulk_in_buffer == NULL) {
-		port->bulk_out_buffer = NULL; /* prevent double free */
+	if (port->bulk_in_buffer == NULL)
 		goto enomem;
-	}
 
-	kfree(port->bulk_out_buffer);
 	port->bulk_out_buffer = kmalloc(URBDATA_SIZE, GFP_KERNEL);
 	if (port->bulk_out_buffer == NULL) {
+		/* the buffer is useless, free it */
 		kfree(port->bulk_in_buffer);
 		port->bulk_in_buffer = NULL;
 		goto enomem;
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 9b887ef64ff..70d135e0cc4 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -1658,6 +1658,32 @@ config FB_PM3
 	  similar boards, 3DLabs Permedia3 Create!, Appian Jeronimo 2000
 	  and maybe other boards.
 
+config FB_CARMINE
+	tristate "Fujitsu carmine frame buffer support"
+	depends on FB && PCI
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  This is the frame buffer device driver for the Fujitsu Carmine chip.
+	  The driver provides two independent frame buffer devices.
+
+choice
+	depends on FB_CARMINE
+	prompt "DRAM timing"
+	default FB_CARMINE_DRAM_EVAL
+
+config FB_CARMINE_DRAM_EVAL
+	bool "Eval board timings"
+	help
+	  Use timings which work on the eval card.
+
+config CARMINE_DRAM_CUSTOM
+	bool "Custom board timings"
+	help
+	  Use custom board timings.
+endchoice
+
 config FB_AU1100
 	bool "Au1100 LCD Driver"
 	depends on (FB = y) && MIPS && SOC_AU1100
@@ -1840,6 +1866,16 @@ config FB_W100
 
 	  If unsure, say N.
 
+config FB_SH_MOBILE_LCDC
+	tristate "SuperH Mobile LCDC framebuffer support"
+	depends on FB && SUPERH
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	default m
+	---help---
+	  Frame buffer driver for the on-chip SH-Mobile LCD controller.
+
 config FB_S3C2410
 	tristate "S3C2410 LCD framebuffer support"
 	depends on FB && ARCH_S3C2410
@@ -1951,6 +1987,23 @@ config FB_AM200EPD
          This enables support for the Metronome display controller used on
          the E-Ink AM-200 EPD devkit.
 
+config FB_COBALT
+	tristate "Cobalt server LCD frame buffer support"
+	depends on FB && MIPS_COBALT
+
+config FB_SH7760
+       bool "SH7760/SH7763 LCDC support"
+       depends on FB && (CPU_SUBTYPE_SH7760 || CPU_SUBTYPE_SH7763)
+       select FB_CFB_FILLRECT
+       select FB_CFB_COPYAREA
+       select FB_CFB_IMAGEBLIT
+       help
+         Support for the SH7760/SH7763 integrated (D)STN/TFT LCD Controller.
+         Supports display resolutions up to 1024x1024 pixel, grayscale and
+         color operation, with depths ranging from 1 bpp to 8 bpp monochrome
+         and 8, 15 or 16 bpp color; 90 degrees clockwise display rotation for
+         panels <= 320 pixel horizontal resolution.
+
 config FB_VIRTUAL
 	tristate "Virtual Frame Buffer support (ONLY FOR TESTING!)"
 	depends on FB
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 04bca35403f..0ebc1bfd251 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -106,17 +106,22 @@ obj-$(CONFIG_FB_PMAGB_B)	  += pmagb-b-fb.o
 obj-$(CONFIG_FB_MAXINE)		  += maxinefb.o
 obj-$(CONFIG_FB_METRONOME)        += metronomefb.o
 obj-$(CONFIG_FB_S1D13XXX)	  += s1d13xxxfb.o
+obj-$(CONFIG_FB_SH7760)		  += sh7760fb.o
 obj-$(CONFIG_FB_IMX)              += imxfb.o
 obj-$(CONFIG_FB_S3C2410)	  += s3c2410fb.o
 obj-$(CONFIG_FB_FSL_DIU)	  += fsl-diu-fb.o
+obj-$(CONFIG_FB_COBALT)           += cobalt_lcdfb.o
 obj-$(CONFIG_FB_PNX4008_DUM)	  += pnx4008/
 obj-$(CONFIG_FB_PNX4008_DUM_RGB)  += pnx4008/
 obj-$(CONFIG_FB_IBM_GXT4500)	  += gxt4500.o
 obj-$(CONFIG_FB_PS3)		  += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.o
 obj-$(CONFIG_FB_XILINX)           += xilinxfb.o
+obj-$(CONFIG_FB_SH_MOBILE_LCDC)	  += sh_mobile_lcdcfb.o
+obj-$(CONFIG_FB_SH7343VOU)	  += sh7343_voufb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-$(CONFIG_XEN_FBDEV_FRONTEND)  += xen-fbfront.o
+obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
diff --git a/drivers/video/acornfb.c b/drivers/video/acornfb.c
index eedb8285e32..017233d0c48 100644
--- a/drivers/video/acornfb.c
+++ b/drivers/video/acornfb.c
@@ -23,6 +23,7 @@
 #include <linux/string.h>
 #include <linux/ctype.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/init.h>
 #include <linux/fb.h>
 #include <linux/platform_device.h>
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index 45c154ade9c..b8e9a8682f2 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -1136,7 +1136,6 @@ static int amifb_ioctl(struct fb_info *info, unsigned int cmd, unsigned long arg
 	 * Interface to the low level console driver
 	 */
 
-int amifb_init(void);
 static void amifb_deinit(void);
 
 	/*
@@ -2048,13 +2047,16 @@ static void amifb_copyarea(struct fb_info *info,
 	width = x2 - dx;
 	height = y2 - dy;
 
+	if (area->sx + dx < area->dx || area->sy + dy < area->dy)
+		return;
+
 	/* update sx,sy */
 	sx = area->sx + (dx - area->dx);
 	sy = area->sy + (dy - area->dy);
 
 	/* the source must be completely inside the virtual screen */
-	if (sx < 0 || sy < 0 || (sx + width) > info->var.xres_virtual ||
-	    (sy + height) > info->var.yres_virtual)
+	if (sx + width > info->var.xres_virtual ||
+			sy + height > info->var.yres_virtual)
 		return;
 
 	if (dy > sy || (dy == sy && dx > sx)) {
@@ -2245,7 +2247,7 @@ static inline void chipfree(void)
 	 * Initialisation
 	 */
 
-int __init amifb_init(void)
+static int __init amifb_init(void)
 {
 	int tag, i, err = 0;
 	u_long chipptr;
@@ -3790,16 +3792,14 @@ static void ami_rebuild_copper(void)
 	}
 }
 
-
-module_init(amifb_init);
-
-#ifdef MODULE
-MODULE_LICENSE("GPL");
-
-void cleanup_module(void)
+static void __exit amifb_exit(void)
 {
 	unregister_framebuffer(&fb_info);
 	amifb_deinit();
 	amifb_video_off();
 }
-#endif /* MODULE */
+
+module_init(amifb_init);
+module_exit(amifb_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/atafb.c b/drivers/video/atafb.c
index fa55d356b53..77eb8b34fbf 100644
--- a/drivers/video/atafb.c
+++ b/drivers/video/atafb.c
@@ -2593,13 +2593,16 @@ static void atafb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
 	width = x2 - dx;
 	height = y2 - dy;
 
+	if (area->sx + dx < area->dx || area->sy + dy < area->dy)
+		return;
+
 	/* update sx,sy */
 	sx = area->sx + (dx - area->dx);
 	sy = area->sy + (dy - area->dy);
 
 	/* the source must be completely inside the virtual screen */
-	if (sx < 0 || sy < 0 || (sx + width) > info->var.xres_virtual ||
-	    (sy + height) > info->var.yres_virtual)
+	if (sx + width > info->var.xres_virtual ||
+			sy + height > info->var.yres_virtual)
 		return;
 
 	if (dy > sy || (dy == sy && dx > sx)) {
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index b004036d408..5b3a15dffb5 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -256,6 +256,20 @@ static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
 	return 0;
 }
 
+static const struct fb_videomode *atmel_lcdfb_choose_mode(struct fb_var_screeninfo *var,
+						     struct fb_info *info)
+{
+	struct fb_videomode varfbmode;
+	const struct fb_videomode *fbmode = NULL;
+
+	fb_var_to_videomode(&varfbmode, var);
+	fbmode = fb_find_nearest_mode(&varfbmode, &info->modelist);
+	if (fbmode)
+		fb_videomode_to_var(var, fbmode);
+	return fbmode;
+}
+
+
 /**
  *      atmel_lcdfb_check_var - Validates a var passed in.
  *      @var: frame buffer variable screen structure
@@ -289,6 +303,15 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
 
 	dev_dbg(dev, "%s:\n", __func__);
+
+	if (!(var->pixclock && var->bits_per_pixel)) {
+		/* choose a suitable mode if possible */
+		if (!atmel_lcdfb_choose_mode(var, info)) {
+			dev_err(dev, "needed value not specified\n");
+			return -EINVAL;
+		}
+	}
+
 	dev_dbg(dev, "  resolution: %ux%u\n", var->xres, var->yres);
 	dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(var->pixclock));
 	dev_dbg(dev, "  bpp:        %u\n", var->bits_per_pixel);
@@ -299,6 +322,13 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 		return -EINVAL;
 	}
 
+	/* Do not allow to have real resoulution larger than virtual */
+	if (var->xres > var->xres_virtual)
+		var->xres_virtual = var->xres;
+
+	if (var->yres > var->yres_virtual)
+		var->yres_virtual = var->yres;
+
 	/* Force same alignment for each line */
 	var->xres = (var->xres + 3) & ~3UL;
 	var->xres_virtual = (var->xres_virtual + 3) & ~3UL;
@@ -379,6 +409,35 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 	return 0;
 }
 
+/*
+ * LCD reset sequence
+ */
+static void atmel_lcdfb_reset(struct atmel_lcdfb_info *sinfo)
+{
+	might_sleep();
+
+	/* LCD power off */
+	lcdc_writel(sinfo, ATMEL_LCDC_PWRCON, sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET);
+
+	/* wait for the LCDC core to become idle */
+	while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
+		msleep(10);
+
+	/* DMA disable */
+	lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);
+
+	/* wait for DMA engine to become idle */
+	while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
+		msleep(10);
+
+	/* LCD power on */
+	lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
+		(sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET) | ATMEL_LCDC_PWR);
+
+	/* DMA enable */
+	lcdc_writel(sinfo, ATMEL_LCDC_DMACON, sinfo->default_dmacon);
+}
+
 /**
  *      atmel_lcdfb_set_par - Alters the hardware state.
  *      @info: frame buffer structure that represents a single frame buffer
@@ -401,6 +460,8 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 	unsigned long clk_value_khz;
 	unsigned long bits_per_line;
 
+	might_sleep();
+
 	dev_dbg(info->device, "%s:\n", __func__);
 	dev_dbg(info->device, "  * resolution: %ux%u (%ux%u virtual)\n",
 		 info->var.xres, info->var.yres,
@@ -511,6 +572,8 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 
 	/* Disable all interrupts */
 	lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
+	/* Enable FIFO & DMA errors */
+	lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
 
 	/* ...wait for DMA engine to become idle... */
 	while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
@@ -645,10 +708,26 @@ static irqreturn_t atmel_lcdfb_interrupt(int irq, void *dev_id)
 	u32 status;
 
 	status = lcdc_readl(sinfo, ATMEL_LCDC_ISR);
-	lcdc_writel(sinfo, ATMEL_LCDC_IDR, status);
+	if (status & ATMEL_LCDC_UFLWI) {
+		dev_warn(info->device, "FIFO underflow %#x\n", status);
+		/* reset DMA and FIFO to avoid screen shifting */
+		schedule_work(&sinfo->task);
+	}
+	lcdc_writel(sinfo, ATMEL_LCDC_ICR, status);
 	return IRQ_HANDLED;
 }
 
+/*
+ * LCD controller task (to reset the LCD)
+ */
+static void atmel_lcdfb_task(struct work_struct *work)
+{
+	struct atmel_lcdfb_info *sinfo =
+		container_of(work, struct atmel_lcdfb_info, task);
+
+	atmel_lcdfb_reset(sinfo);
+}
+
 static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
 {
 	struct fb_info *info = sinfo->info;
@@ -691,6 +770,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	struct fb_info *info;
 	struct atmel_lcdfb_info *sinfo;
 	struct atmel_lcdfb_info *pdata_sinfo;
+	struct fb_videomode fbmode;
 	struct resource *regs = NULL;
 	struct resource *map = NULL;
 	int ret;
@@ -824,6 +904,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 		goto unmap_mmio;
 	}
 
+	/* Some operations on the LCDC might sleep and
+	 * require a preemptible task context */
+	INIT_WORK(&sinfo->task, atmel_lcdfb_task);
+
 	ret = atmel_lcdfb_init_fbinfo(sinfo);
 	if (ret < 0) {
 		dev_err(dev, "init fbinfo failed: %d\n", ret);
@@ -853,6 +937,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 		goto free_cmap;
 	}
 
+	/* add selected videomode to modelist */
+	fb_var_to_videomode(&fbmode, &info->var);
+	fb_add_videomode(&fbmode, &info->modelist);
+
 	/* Power up the LCDC screen */
 	if (sinfo->atmel_lcdfb_power_control)
 		sinfo->atmel_lcdfb_power_control(1);
@@ -866,6 +954,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 free_cmap:
 	fb_dealloc_cmap(&info->cmap);
 unregister_irqs:
+	cancel_work_sync(&sinfo->task);
 	free_irq(sinfo->irq_base, info);
 unmap_mmio:
 	exit_backlight(sinfo);
@@ -903,6 +992,7 @@ static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
 	if (!sinfo)
 		return 0;
 
+	cancel_work_sync(&sinfo->task);
 	exit_backlight(sinfo);
 	if (sinfo->atmel_lcdfb_power_control)
 		sinfo->atmel_lcdfb_power_control(0);
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index 07b6addbb3c..243ea4ab20c 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -1339,10 +1339,8 @@ static int aty128_var_to_pll(u32 period_in_ps, struct aty128_pll *pll,
 	if (vclk * 12 < c.ppll_min)
 		vclk = c.ppll_min/12;
 
-	pll->post_divider = -1;
-
 	/* now, find an acceptable divider */
-	for (i = 0; i < sizeof(post_dividers); i++) {
+	for (i = 0; i < ARRAY_SIZE(post_dividers); i++) {
 		output_freq = post_dividers[i] * vclk;
 		if (output_freq >= c.ppll_min && output_freq <= c.ppll_max) {
 			pll->post_divider = post_dividers[i];
@@ -1350,7 +1348,7 @@ static int aty128_var_to_pll(u32 period_in_ps, struct aty128_pll *pll,
 		}
 	}
 
-	if (pll->post_divider < 0)
+	if (i == ARRAY_SIZE(post_dividers))
 		return -EINVAL;
 
 	/* calculate feedback divider */
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index bd4ac0bafec..620ba812036 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -424,7 +424,6 @@ static struct {
 #endif /* CONFIG_FB_ATY_CT */
 };
 
-/* can not fail */
 static int __devinit correct_chipset(struct atyfb_par *par)
 {
 	u8 rev;
@@ -437,6 +436,9 @@ static int __devinit correct_chipset(struct atyfb_par *par)
 		if (par->pci_id == aty_chips[i].pci_id)
 			break;
 
+	if (i < 0)
+		return -ENODEV;
+
 	name = aty_chips[i].name;
 	par->pll_limits.pll_max = aty_chips[i].pll;
 	par->pll_limits.mclk = aty_chips[i].mclk;
@@ -2229,6 +2231,7 @@ static int __devinit aty_init(struct fb_info *info)
 	const char *ramname = NULL, *xtal;
 	int gtb_memsize, has_var = 0;
 	struct fb_var_screeninfo var;
+	int ret;
 
 	init_waitqueue_head(&par->vblank.wait);
 	spin_lock_init(&par->int_lock);
@@ -2610,7 +2613,8 @@ static int __devinit aty_init(struct fb_info *info)
 			var.yres_virtual = var.yres;
 	}
 
-	if (atyfb_check_var(&var, info)) {
+	ret = atyfb_check_var(&var, info);
+	if (ret) {
 		PRINTKE("can't set default video mode\n");
 		goto aty_init_exit;
 	}
@@ -2621,10 +2625,12 @@ static int __devinit aty_init(struct fb_info *info)
 #endif /* CONFIG_FB_ATY_CT */
 	info->var = var;
 
-	if (fb_alloc_cmap(&info->cmap, 256, 0) < 0)
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret < 0)
 		goto aty_init_exit;
 
-	if (register_framebuffer(info) < 0) {
+	ret = register_framebuffer(info);
+	if (ret < 0) {
 		fb_dealloc_cmap(&info->cmap);
 		goto aty_init_exit;
 	}
@@ -2650,7 +2656,7 @@ aty_init_exit:
 	    par->mtrr_aper = -1;
 	}
 #endif
-	return -1;
+	return ret;
 }
 
 static void aty_resume_chip(struct fb_info *info)
@@ -2709,8 +2715,7 @@ static int atyfb_blank(int blank, struct fb_info *info)
 	if (par->lock_blank || par->asleep)
 		return 0;
 
-#ifdef CONFIG_FB_ATY_BACKLIGHT
-#elif defined(CONFIG_FB_ATY_GENERIC_LCD)
+#ifdef CONFIG_FB_ATY_GENERIC_LCD
 	if (par->lcd_table && blank > FB_BLANK_NORMAL &&
 	    (aty_ld_lcd(LCD_GEN_CNTL, par) & LCD_ON)) {
 		u32 pm = aty_ld_lcd(POWER_MANAGEMENT, par);
@@ -2739,8 +2744,7 @@ static int atyfb_blank(int blank, struct fb_info *info)
 	}
 	aty_st_le32(CRTC_GEN_CNTL, gen_cntl, par);
 
-#ifdef CONFIG_FB_ATY_BACKLIGHT
-#elif defined(CONFIG_FB_ATY_GENERIC_LCD)
+#ifdef CONFIG_FB_ATY_GENERIC_LCD
 	if (par->lcd_table && blank <= FB_BLANK_NORMAL &&
 	    (aty_ld_lcd(LCD_GEN_CNTL, par) & LCD_ON)) {
 		u32 pm = aty_ld_lcd(POWER_MANAGEMENT, par);
@@ -3331,7 +3335,7 @@ static int __devinit init_from_bios(struct atyfb_par *par)
 		PRINTKE("no BIOS frequency table found, use parameters\n");
 		ret = -ENXIO;
 	}
-	iounmap((void* __iomem )bios_base);
+	iounmap((void __iomem *)bios_base);
 
 	return ret;
 }
@@ -3418,14 +3422,7 @@ static int __devinit atyfb_pci_probe(struct pci_dev *pdev, const struct pci_devi
 	struct fb_info *info;
 	struct resource *rp;
 	struct atyfb_par *par;
-	int i, rc = -ENOMEM;
-
-	for (i = ARRAY_SIZE(aty_chips) - 1; i >= 0; i--)
-		if (pdev->device == aty_chips[i].pci_id)
-			break;
-
-	if (i < 0)
-		return -ENODEV;
+	int rc = -ENOMEM;
 
 	/* Enable device in PCI config */
 	if (pci_enable_device(pdev)) {
@@ -3456,7 +3453,7 @@ static int __devinit atyfb_pci_probe(struct pci_dev *pdev, const struct pci_devi
 	par = info->par;
 	info->fix = atyfb_fix;
 	info->device = &pdev->dev;
-	par->pci_id = aty_chips[i].pci_id;
+	par->pci_id = pdev->device;
 	par->res_start = res_start;
 	par->res_size = res_size;
 	par->irq = pdev->irq;
@@ -3474,7 +3471,8 @@ static int __devinit atyfb_pci_probe(struct pci_dev *pdev, const struct pci_devi
 	pci_set_drvdata(pdev, info);
 
 	/* Init chip & register framebuffer */
-	if (aty_init(info))
+	rc = aty_init(info);
+	if (rc)
 		goto err_release_io;
 
 #ifdef __sparc__
@@ -3655,18 +3653,62 @@ static void __devexit atyfb_pci_remove(struct pci_dev *pdev)
 	atyfb_remove(info);
 }
 
-/*
- * This driver uses its own matching table. That will be more difficult
- * to fix, so for now, we just match against any ATI ID and let the
- * probe() function find out what's up. That also mean we don't have
- * a module ID table though.
- */
 static struct pci_device_id atyfb_pci_tbl[] = {
-	{ PCI_VENDOR_ID_ATI, PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID,
-	  PCI_BASE_CLASS_DISPLAY << 16, 0xff0000, 0 },
-	{ 0, }
+#ifdef CONFIG_FB_ATY_GX
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GX) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64CX) },
+#endif /* CONFIG_FB_ATY_GX */
+
+#ifdef CONFIG_FB_ATY_CT
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64CT) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64ET) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LT) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64VT) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GT) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64VU) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GU) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LG) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64VV) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GV) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GW) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GY) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GZ) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GB) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GD) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GI) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GP) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GQ) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LB) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LD) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LI) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LP) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LQ) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GM) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GN) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GO) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GR) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64GS) },
+
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LM) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LN) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LR) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_CHIP_MACH64LS) },
+#endif /* CONFIG_FB_ATY_CT */
+	{ }
 };
 
+MODULE_DEVICE_TABLE(pci, atyfb_pci_tbl);
+
 static struct pci_driver atyfb_driver = {
 	.name		= "atyfb",
 	.id_table	= atyfb_pci_tbl,
diff --git a/drivers/video/aty/radeon_base.c b/drivers/video/aty/radeon_base.c
index 400e9264e45..652273e9f5f 100644
--- a/drivers/video/aty/radeon_base.c
+++ b/drivers/video/aty/radeon_base.c
@@ -2098,15 +2098,7 @@ static void radeon_identify_vram(struct radeonfb_info *rinfo)
 
 static ssize_t radeon_show_one_edid(char *buf, loff_t off, size_t count, const u8 *edid)
 {
-	if (off > EDID_LENGTH)
-		return 0;
-
-	if (off + count > EDID_LENGTH)
-		count = EDID_LENGTH - off;
-
-	memcpy(buf, edid + off, count);
-
-	return count;
+	return memory_read_from_buffer(buf, count, &off, edid, EDID_LENGTH);
 }
 
 
@@ -2161,6 +2153,7 @@ static int __devinit radeonfb_pci_register (struct pci_dev *pdev,
 	struct radeonfb_info *rinfo;
 	int ret;
 	unsigned char c1, c2;
+	int err = 0;
 
 	pr_debug("radeonfb_pci_register BEGIN\n");
 	
@@ -2340,9 +2333,14 @@ static int __devinit radeonfb_pci_register (struct pci_dev *pdev,
 
 	/* Register some sysfs stuff (should be done better) */
 	if (rinfo->mon1_EDID)
-		sysfs_create_bin_file(&rinfo->pdev->dev.kobj, &edid1_attr);
+		err |= sysfs_create_bin_file(&rinfo->pdev->dev.kobj,
+						&edid1_attr);
 	if (rinfo->mon2_EDID)
-		sysfs_create_bin_file(&rinfo->pdev->dev.kobj, &edid2_attr);
+		err |= sysfs_create_bin_file(&rinfo->pdev->dev.kobj,
+						&edid2_attr);
+	if (err)
+		pr_warning("%s() Creating sysfs files failed, continuing\n",
+			   __func__);
 
 	/* save current mode regs before we switch into the new one
 	 * so we can restore this upon __exit
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 30bf7f2f163..452b770d8cc 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -36,6 +36,30 @@ config LCD_LTV350QV
 
 	  The LTV350QV panel is present on all ATSTK1000 boards.
 
+config LCD_ILI9320
+	tristate
+	depends on LCD_CLASS_DEVICE && BACKLIGHT_LCD_SUPPORT
+	default n
+	help
+	  If you have a panel based on the ILI9320 controller chip
+	  then say y to include a power driver for it.
+
+config LCD_VGG2432A4
+	tristate "VGG2432A4 LCM device support"
+	depends on BACKLIGHT_LCD_SUPPORT && LCD_CLASS_DEVICE && SPI_MASTER
+	select LCD_ILI9320
+	default n
+	help
+	  If you have a VGG2432A4 panel based on the ILI9320 controller chip
+	  then say y to include a power driver for it.
+
+config LCD_PLATFORM
+	tristate "Platform LCD controls"
+	depends on LCD_CLASS_DEVICE
+	help
+	  This driver provides a platform-device registered LCD power
+	  control interface.
+
 #
 # Backlight
 #
@@ -63,6 +87,18 @@ config BACKLIGHT_ATMEL_LCDC
 	  If in doubt, it's safe to enable this option; it doesn't kick
 	  in unless the board's description says it's wired that way.
 
+config BACKLIGHT_ATMEL_PWM
+	tristate "Atmel PWM backlight control"
+	depends on BACKLIGHT_CLASS_DEVICE && ATMEL_PWM
+	default n
+	help
+	  Say Y here if you want to use the PWM peripheral in Atmel AT91 and
+	  AVR32 devices. This driver will need additional platform data to know
+	  which PWM instance to use and how to configure it.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called atmel-pwm-bl.
+
 config BACKLIGHT_CORGI
 	tristate "Generic (aka Sharp Corgi) Backlight Driver"
 	depends on BACKLIGHT_CLASS_DEVICE
@@ -119,3 +155,12 @@ config BACKLIGHT_PWM
 	help
 	  If you have a LCD backlight adjustable by PWM, say Y to enable
 	  this driver.
+
+config BACKLIGHT_MBP_NVIDIA
+       tristate "MacBook Pro Nvidia Backlight Driver"
+       depends on BACKLIGHT_CLASS_DEVICE && X86
+       default n
+       help
+         If you have an Apple Macbook Pro with Nvidia graphics hardware say Y
+	 to enable a driver for its backlight
+
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index b51a7cd1250..b405aace803 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -1,9 +1,13 @@
 # Backlight & LCD drivers
 
 obj-$(CONFIG_LCD_CLASS_DEVICE)     += lcd.o
-obj-$(CONFIG_LCD_LTV350QV)	+= ltv350qv.o
+obj-$(CONFIG_LCD_LTV350QV)	   += ltv350qv.o
+obj-$(CONFIG_LCD_ILI9320)	   += ili9320.o
+obj-$(CONFIG_LCD_PLATFORM)	   += platform_lcd.o
+obj-$(CONFIG_LCD_VGG2432A4)	   += vgg2432a4.o
 
 obj-$(CONFIG_BACKLIGHT_CLASS_DEVICE) += backlight.o
+obj-$(CONFIG_BACKLIGHT_ATMEL_PWM)    += atmel-pwm-bl.o
 obj-$(CONFIG_BACKLIGHT_CORGI)	+= corgi_bl.o
 obj-$(CONFIG_BACKLIGHT_HP680)	+= hp680_bl.o
 obj-$(CONFIG_BACKLIGHT_LOCOMO)	+= locomolcd.o
@@ -11,3 +15,5 @@ obj-$(CONFIG_BACKLIGHT_OMAP1)	+= omap1_bl.o
 obj-$(CONFIG_BACKLIGHT_PROGEAR) += progear_bl.o
 obj-$(CONFIG_BACKLIGHT_CARILLO_RANCH) += cr_bllcd.o
 obj-$(CONFIG_BACKLIGHT_PWM)	+= pwm_bl.o
+obj-$(CONFIG_BACKLIGHT_MBP_NVIDIA) += mbp_nvidia_bl.o
+
diff --git a/drivers/video/backlight/atmel-pwm-bl.c b/drivers/video/backlight/atmel-pwm-bl.c
new file mode 100644
index 00000000000..505c0823a10
--- /dev/null
+++ b/drivers/video/backlight/atmel-pwm-bl.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) 2008 Atmel Corporation
+ *
+ * Backlight driver using Atmel PWM peripheral.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/backlight.h>
+#include <linux/atmel_pwm.h>
+#include <linux/atmel-pwm-bl.h>
+
+struct atmel_pwm_bl {
+	const struct atmel_pwm_bl_platform_data	*pdata;
+	struct backlight_device			*bldev;
+	struct platform_device			*pdev;
+	struct pwm_channel			pwmc;
+	int					gpio_on;
+};
+
+static int atmel_pwm_bl_set_intensity(struct backlight_device *bd)
+{
+	struct atmel_pwm_bl *pwmbl = bl_get_data(bd);
+	int intensity = bd->props.brightness;
+	int pwm_duty;
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		intensity = 0;
+
+	if (pwmbl->pdata->pwm_active_low)
+		pwm_duty = pwmbl->pdata->pwm_duty_min + intensity;
+	else
+		pwm_duty = pwmbl->pdata->pwm_duty_max - intensity;
+
+	if (pwm_duty > pwmbl->pdata->pwm_duty_max)
+		pwm_duty = pwmbl->pdata->pwm_duty_max;
+	if (pwm_duty < pwmbl->pdata->pwm_duty_min)
+		pwm_duty = pwmbl->pdata->pwm_duty_min;
+
+	if (!intensity) {
+		if (pwmbl->gpio_on != -1) {
+			gpio_set_value(pwmbl->gpio_on,
+					0 ^ pwmbl->pdata->on_active_low);
+		}
+		pwm_channel_writel(&pwmbl->pwmc, PWM_CUPD, pwm_duty);
+		pwm_channel_disable(&pwmbl->pwmc);
+	} else {
+		pwm_channel_enable(&pwmbl->pwmc);
+		pwm_channel_writel(&pwmbl->pwmc, PWM_CUPD, pwm_duty);
+		if (pwmbl->gpio_on != -1) {
+			gpio_set_value(pwmbl->gpio_on,
+					1 ^ pwmbl->pdata->on_active_low);
+		}
+	}
+
+	return 0;
+}
+
+static int atmel_pwm_bl_get_intensity(struct backlight_device *bd)
+{
+	struct atmel_pwm_bl *pwmbl = bl_get_data(bd);
+	u8 intensity;
+
+	if (pwmbl->pdata->pwm_active_low) {
+		intensity = pwm_channel_readl(&pwmbl->pwmc, PWM_CDTY) -
+			pwmbl->pdata->pwm_duty_min;
+	} else {
+		intensity = pwmbl->pdata->pwm_duty_max -
+			pwm_channel_readl(&pwmbl->pwmc, PWM_CDTY);
+	}
+
+	return intensity;
+}
+
+static int atmel_pwm_bl_init_pwm(struct atmel_pwm_bl *pwmbl)
+{
+	unsigned long pwm_rate = pwmbl->pwmc.mck;
+	unsigned long prescale = DIV_ROUND_UP(pwm_rate,
+			(pwmbl->pdata->pwm_frequency *
+			 pwmbl->pdata->pwm_compare_max)) - 1;
+
+	/*
+	 * Prescale must be power of two and maximum 0xf in size because of
+	 * hardware limit. PWM speed will be:
+	 *	PWM module clock speed / (2 ^ prescale).
+	 */
+	prescale = fls(prescale);
+	if (prescale > 0xf)
+		prescale = 0xf;
+
+	pwm_channel_writel(&pwmbl->pwmc, PWM_CMR, prescale);
+	pwm_channel_writel(&pwmbl->pwmc, PWM_CDTY,
+			pwmbl->pdata->pwm_duty_min +
+			pwmbl->bldev->props.brightness);
+	pwm_channel_writel(&pwmbl->pwmc, PWM_CPRD,
+			pwmbl->pdata->pwm_compare_max);
+
+	dev_info(&pwmbl->pdev->dev, "Atmel PWM backlight driver "
+			"(%lu Hz)\n", pwmbl->pwmc.mck /
+			pwmbl->pdata->pwm_compare_max /
+			(1 << prescale));
+
+	return pwm_channel_enable(&pwmbl->pwmc);
+}
+
+static struct backlight_ops atmel_pwm_bl_ops = {
+	.get_brightness = atmel_pwm_bl_get_intensity,
+	.update_status  = atmel_pwm_bl_set_intensity,
+};
+
+static int atmel_pwm_bl_probe(struct platform_device *pdev)
+{
+	const struct atmel_pwm_bl_platform_data *pdata;
+	struct backlight_device *bldev;
+	struct atmel_pwm_bl *pwmbl;
+	int retval;
+
+	pwmbl = kzalloc(sizeof(struct atmel_pwm_bl), GFP_KERNEL);
+	if (!pwmbl)
+		return -ENOMEM;
+
+	pwmbl->pdev = pdev;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		retval = -ENODEV;
+		goto err_free_mem;
+	}
+
+	if (pdata->pwm_compare_max < pdata->pwm_duty_max ||
+			pdata->pwm_duty_min > pdata->pwm_duty_max ||
+			pdata->pwm_frequency == 0) {
+		retval = -EINVAL;
+		goto err_free_mem;
+	}
+
+	pwmbl->pdata = pdata;
+	pwmbl->gpio_on = pdata->gpio_on;
+
+	retval = pwm_channel_alloc(pdata->pwm_channel, &pwmbl->pwmc);
+	if (retval)
+		goto err_free_mem;
+
+	if (pwmbl->gpio_on != -1) {
+		retval = gpio_request(pwmbl->gpio_on, "gpio_atmel_pwm_bl");
+		if (retval) {
+			pwmbl->gpio_on = -1;
+			goto err_free_pwm;
+		}
+
+		/* Turn display off by defatult. */
+		retval = gpio_direction_output(pwmbl->gpio_on,
+				0 ^ pdata->on_active_low);
+		if (retval)
+			goto err_free_gpio;
+	}
+
+	bldev = backlight_device_register("atmel-pwm-bl",
+			&pdev->dev, pwmbl, &atmel_pwm_bl_ops);
+	if (IS_ERR(bldev)) {
+		retval = PTR_ERR(bldev);
+		goto err_free_gpio;
+	}
+
+	pwmbl->bldev = bldev;
+
+	platform_set_drvdata(pdev, pwmbl);
+
+	/* Power up the backlight by default at middle intesity. */
+	bldev->props.power = FB_BLANK_UNBLANK;
+	bldev->props.max_brightness = pdata->pwm_duty_max - pdata->pwm_duty_min;
+	bldev->props.brightness = bldev->props.max_brightness / 2;
+
+	retval = atmel_pwm_bl_init_pwm(pwmbl);
+	if (retval)
+		goto err_free_bl_dev;
+
+	atmel_pwm_bl_set_intensity(bldev);
+
+	return 0;
+
+err_free_bl_dev:
+	platform_set_drvdata(pdev, NULL);
+	backlight_device_unregister(bldev);
+err_free_gpio:
+	if (pwmbl->gpio_on != -1)
+		gpio_free(pwmbl->gpio_on);
+err_free_pwm:
+	pwm_channel_free(&pwmbl->pwmc);
+err_free_mem:
+	kfree(pwmbl);
+	return retval;
+}
+
+static int __exit atmel_pwm_bl_remove(struct platform_device *pdev)
+{
+	struct atmel_pwm_bl *pwmbl = platform_get_drvdata(pdev);
+
+	if (pwmbl->gpio_on != -1) {
+		gpio_set_value(pwmbl->gpio_on, 0);
+		gpio_free(pwmbl->gpio_on);
+	}
+	pwm_channel_disable(&pwmbl->pwmc);
+	pwm_channel_free(&pwmbl->pwmc);
+	backlight_device_unregister(pwmbl->bldev);
+	platform_set_drvdata(pdev, NULL);
+	kfree(pwmbl);
+
+	return 0;
+}
+
+static struct platform_driver atmel_pwm_bl_driver = {
+	.driver = {
+		.name = "atmel-pwm-bl",
+	},
+	/* REVISIT add suspend() and resume() */
+	.remove = __exit_p(atmel_pwm_bl_remove),
+};
+
+static int __init atmel_pwm_bl_init(void)
+{
+	return platform_driver_probe(&atmel_pwm_bl_driver, atmel_pwm_bl_probe);
+}
+module_init(atmel_pwm_bl_init);
+
+static void __exit atmel_pwm_bl_exit(void)
+{
+	platform_driver_unregister(&atmel_pwm_bl_driver);
+}
+module_exit(atmel_pwm_bl_exit);
+
+MODULE_AUTHOR("Hans-Christian egtvedt <hans-christian.egtvedt@atmel.com>");
+MODULE_DESCRIPTION("Atmel PWM backlight driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index 39394757679..fab0bc874b5 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -191,6 +191,7 @@ static struct device_attribute bl_device_attributes[] = {
  *   backlight_device class.
  * @name: the name of the new object(must be the same as the name of the
  *   respective framebuffer device).
+ * @parent: a pointer to the parent device
  * @devdata: an optional pointer to be stored for private driver use. The
  *   methods may retrieve it by using bl_get_data(bd).
  * @ops: the backlight operations structure.
diff --git a/drivers/video/backlight/ili9320.c b/drivers/video/backlight/ili9320.c
new file mode 100644
index 00000000000..ba89b41b639
--- /dev/null
+++ b/drivers/video/backlight/ili9320.c
@@ -0,0 +1,330 @@
+/* drivers/video/backlight/ili9320.c
+ *
+ * ILI9320 LCD controller driver core.
+ *
+ * Copyright 2007 Simtec Electronics
+ *	http://armlinux.simtec.co.uk/
+ *	Ben Dooks <ben@simtec.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/delay.h>
+#include <linux/err.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/lcd.h>
+#include <linux/module.h>
+
+#include <linux/spi/spi.h>
+
+#include <video/ili9320.h>
+
+#include "ili9320.h"
+
+
+static inline int ili9320_write_spi(struct ili9320 *ili,
+				    unsigned int reg,
+				    unsigned int value)
+{
+	struct ili9320_spi *spi = &ili->access.spi;
+	unsigned char *addr = spi->buffer_addr;
+	unsigned char *data = spi->buffer_data;
+
+	/* spi message consits of:
+	 * first byte: ID and operation
+	 */
+
+	addr[0] = spi->id | ILI9320_SPI_INDEX | ILI9320_SPI_WRITE;
+	addr[1] = reg >> 8;
+	addr[2] = reg;
+
+	/* second message is the data to transfer */
+
+	data[0] = spi->id | ILI9320_SPI_DATA  | ILI9320_SPI_WRITE;
+ 	data[1] = value >> 8;
+	data[2] = value;
+
+	return spi_sync(spi->dev, &spi->message);
+}
+
+int ili9320_write(struct ili9320 *ili, unsigned int reg, unsigned int value)
+{
+	dev_dbg(ili->dev, "write: reg=%02x, val=%04x\n", reg, value);
+	return ili->write(ili, reg, value);
+}
+
+EXPORT_SYMBOL_GPL(ili9320_write);
+
+int ili9320_write_regs(struct ili9320 *ili,
+		       struct ili9320_reg *values,
+		       int nr_values)
+{
+	int index;
+	int ret;
+
+	for (index = 0; index < nr_values; index++, values++) {
+		ret = ili9320_write(ili, values->address, values->value);
+		if (ret != 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+EXPORT_SYMBOL_GPL(ili9320_write_regs);
+
+static void ili9320_reset(struct ili9320 *lcd)
+{
+	struct ili9320_platdata *cfg = lcd->platdata;
+
+	cfg->reset(1);
+	mdelay(50);
+
+	cfg->reset(0);
+	mdelay(50);
+
+	cfg->reset(1);
+	mdelay(100);
+}
+
+static inline int ili9320_init_chip(struct ili9320 *lcd)
+{
+	int ret;
+
+	ili9320_reset(lcd);
+
+	ret = lcd->client->init(lcd, lcd->platdata);
+	if (ret != 0) {
+		dev_err(lcd->dev, "failed to initialise display\n");
+		return ret;
+	}
+
+	lcd->initialised = 1;
+	return 0;
+}
+
+static inline int ili9320_power_on(struct ili9320 *lcd)
+{
+	if (!lcd->initialised)
+		ili9320_init_chip(lcd);
+
+	lcd->display1 |= (ILI9320_DISPLAY1_D(3) | ILI9320_DISPLAY1_BASEE);
+	ili9320_write(lcd, ILI9320_DISPLAY1, lcd->display1);
+
+	return 0;
+}
+
+static inline int ili9320_power_off(struct ili9320 *lcd)
+{
+	lcd->display1 &= ~(ILI9320_DISPLAY1_D(3) | ILI9320_DISPLAY1_BASEE);
+	ili9320_write(lcd, ILI9320_DISPLAY1, lcd->display1);
+
+	return 0;
+}
+
+#define POWER_IS_ON(pwr)	((pwr) <= FB_BLANK_NORMAL)
+
+static int ili9320_power(struct ili9320 *lcd, int power)
+{
+	int ret = 0;
+
+	dev_dbg(lcd->dev, "power %d => %d\n", lcd->power, power);
+
+	if (POWER_IS_ON(power) && !POWER_IS_ON(lcd->power))
+		ret = ili9320_power_on(lcd);
+	else if (!POWER_IS_ON(power) && POWER_IS_ON(lcd->power))
+		ret = ili9320_power_off(lcd);
+
+	if (ret == 0)
+		lcd->power = power;
+	else
+		dev_warn(lcd->dev, "failed to set power mode %d\n", power);
+
+	return ret;
+}
+
+static inline struct ili9320 *to_our_lcd(struct lcd_device *lcd)
+{
+	return lcd_get_data(lcd);
+}
+
+static int ili9320_set_power(struct lcd_device *ld, int power)
+{
+	struct ili9320 *lcd = to_our_lcd(ld);
+
+	return ili9320_power(lcd, power);
+}
+
+static int ili9320_get_power(struct lcd_device *ld)
+{
+	struct ili9320 *lcd = to_our_lcd(ld);
+
+	return lcd->power;
+}
+
+static struct lcd_ops ili9320_ops = {
+	.get_power	= ili9320_get_power,
+	.set_power	= ili9320_set_power,
+};
+
+static void __devinit ili9320_setup_spi(struct ili9320 *ili,
+					struct spi_device *dev)
+{
+	struct ili9320_spi *spi = &ili->access.spi;
+
+	ili->write = ili9320_write_spi;
+	spi->dev = dev;
+
+	/* fill the two messages we are going to use to send the data
+	 * with, the first the address followed by the data. The datasheet
+	 * says they should be done as two distinct cycles of the SPI CS line.
+	 */
+
+	spi->xfer[0].tx_buf = spi->buffer_addr;
+	spi->xfer[1].tx_buf = spi->buffer_data;
+	spi->xfer[0].len = 3;
+	spi->xfer[1].len = 3;
+	spi->xfer[0].bits_per_word = 8;
+	spi->xfer[1].bits_per_word = 8;
+	spi->xfer[0].cs_change = 1;
+
+	spi_message_init(&spi->message);
+	spi_message_add_tail(&spi->xfer[0], &spi->message);
+	spi_message_add_tail(&spi->xfer[1], &spi->message);
+}
+
+int __devinit ili9320_probe_spi(struct spi_device *spi,
+				struct ili9320_client *client)
+{
+	struct ili9320_platdata *cfg = spi->dev.platform_data;
+	struct device *dev = &spi->dev;
+	struct ili9320 *ili;
+	struct lcd_device *lcd;
+	int ret = 0;
+
+	/* verify we where given some information */
+
+	if (cfg == NULL) {
+		dev_err(dev, "no platform data supplied\n");
+		return -EINVAL;
+	}
+
+	if (cfg->hsize <= 0 || cfg->vsize <= 0 || cfg->reset == NULL) {
+		dev_err(dev, "invalid platform data supplied\n");
+		return -EINVAL;
+	}
+
+	/* allocate and initialse our state */
+
+	ili = kzalloc(sizeof(struct ili9320), GFP_KERNEL);
+	if (ili == NULL) {
+		dev_err(dev, "no memory for device\n");
+		return -ENOMEM;
+	}
+
+	ili->access.spi.id = ILI9320_SPI_IDCODE | ILI9320_SPI_ID(1);
+
+	ili->dev = dev;
+	ili->client = client;
+	ili->power = FB_BLANK_POWERDOWN;
+	ili->platdata = cfg;
+
+	dev_set_drvdata(&spi->dev, ili);
+
+	ili9320_setup_spi(ili, spi);
+
+	lcd = lcd_device_register("ili9320", dev, ili, &ili9320_ops);
+	if (IS_ERR(lcd)) {
+		dev_err(dev, "failed to register lcd device\n");
+		ret = PTR_ERR(lcd);
+		goto err_free;
+	}
+
+	ili->lcd = lcd;
+
+	dev_info(dev, "initialising %s\n", client->name);
+
+	ret = ili9320_power(ili, FB_BLANK_UNBLANK);
+	if (ret != 0) {
+		dev_err(dev, "failed to set lcd power state\n");
+		goto err_unregister;
+	}
+
+	return 0;
+
+ err_unregister:
+	lcd_device_unregister(lcd);
+
+ err_free:
+	kfree(ili);
+
+	return ret;
+}
+
+EXPORT_SYMBOL_GPL(ili9320_probe_spi);
+
+int __devexit ili9320_remove(struct ili9320 *ili)
+{
+	ili9320_power(ili, FB_BLANK_POWERDOWN);
+
+	lcd_device_unregister(ili->lcd);
+	kfree(ili);
+
+	return 0;
+}
+
+EXPORT_SYMBOL_GPL(ili9320_remove);
+
+#ifdef CONFIG_PM
+int ili9320_suspend(struct ili9320 *lcd, pm_message_t state)
+{
+	int ret;
+
+	dev_dbg(lcd->dev, "%s: event %d\n", __func__, state.event);
+
+	if (state.event == PM_EVENT_SUSPEND) {
+		ret = ili9320_power(lcd, FB_BLANK_POWERDOWN);
+
+		if (lcd->platdata->suspend == ILI9320_SUSPEND_DEEP) {
+			ili9320_write(lcd, ILI9320_POWER1, lcd->power1 |
+				      ILI9320_POWER1_SLP |
+				      ILI9320_POWER1_DSTB);
+			lcd->initialised = 0;
+		}
+
+		return ret;
+	}
+
+	return 0;
+}
+
+EXPORT_SYMBOL_GPL(ili9320_suspend);
+
+int ili9320_resume(struct ili9320 *lcd)
+{
+	dev_info(lcd->dev, "resuming from power state %d\n", lcd->power);
+
+	if (lcd->platdata->suspend == ILI9320_SUSPEND_DEEP) {
+		ili9320_write(lcd, ILI9320_POWER1, 0x00);
+	}
+
+	return ili9320_power(lcd, FB_BLANK_UNBLANK);
+}
+
+EXPORT_SYMBOL_GPL(ili9320_resume);
+#endif
+
+/* Power down all displays on reboot, poweroff or halt */
+void ili9320_shutdown(struct ili9320 *lcd)
+{
+	ili9320_power(lcd, FB_BLANK_POWERDOWN);
+}
+
+EXPORT_SYMBOL_GPL(ili9320_shutdown);
+
+MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
+MODULE_DESCRIPTION("ILI9320 LCD Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/backlight/ili9320.h b/drivers/video/backlight/ili9320.h
new file mode 100644
index 00000000000..e388eca7cac
--- /dev/null
+++ b/drivers/video/backlight/ili9320.h
@@ -0,0 +1,80 @@
+/* drivers/video/backlight/ili9320.h
+ *
+ * ILI9320 LCD controller driver core.
+ *
+ * Copyright 2007 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>
+ *
+ * http://armlinux.simtec.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.
+*/
+
+/* Holder for register and value pairs. */
+struct ili9320_reg {
+	unsigned short		address;
+	unsigned short		value;
+};
+
+struct ili9320;
+
+struct ili9320_client {
+	const char	*name;
+	int	(*init)(struct ili9320 *ili, struct ili9320_platdata *cfg);
+
+};
+/* Device attached via an SPI bus. */
+struct  ili9320_spi {
+	struct spi_device	*dev;
+	struct spi_message	message;
+	struct spi_transfer	xfer[2];
+
+	unsigned char		id;
+	unsigned char		buffer_addr[4];
+	unsigned char		buffer_data[4];
+};
+
+/* ILI9320 device state. */
+struct ili9320 {
+	union {
+		struct ili9320_spi	spi;	/* SPI attachged device. */
+	} access;				/* Register access method. */
+
+	struct device			*dev;
+	struct lcd_device		*lcd;	/* LCD device we created. */
+	struct ili9320_client		*client;
+	struct ili9320_platdata		*platdata;
+
+	int				 power; /* current power state. */
+	int				 initialised;
+
+	unsigned short			 display1;
+	unsigned short			 power1;
+
+	int (*write)(struct ili9320 *ili, unsigned int reg, unsigned int val);
+};
+
+
+/* ILI9320 register access routines */
+
+extern int ili9320_write(struct ili9320 *ili,
+			 unsigned int reg, unsigned int value);
+
+extern int ili9320_write_regs(struct ili9320 *ili,
+			      struct ili9320_reg *values,
+			      int nr_values);
+
+/* Device probe */
+
+extern int ili9320_probe_spi(struct spi_device *spi,
+			     struct ili9320_client *cli);
+
+extern int ili9320_remove(struct ili9320 *lcd);
+extern void ili9320_shutdown(struct ili9320 *lcd);
+
+/* PM */
+
+extern int ili9320_suspend(struct ili9320 *lcd, pm_message_t state);
+extern int ili9320_resume(struct ili9320 *lcd);
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 299fd318dd4..b15b2b84a6f 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -33,7 +33,7 @@ static int fb_notifier_callback(struct notifier_block *self,
 	ld = container_of(self, struct lcd_device, fb_notif);
 	mutex_lock(&ld->ops_lock);
 	if (ld->ops)
-		if (!ld->ops->check_fb || ld->ops->check_fb(evdata->info))
+		if (!ld->ops->check_fb || ld->ops->check_fb(ld, evdata->info))
 			ld->ops->set_power(ld, *(int *)evdata->data);
 	mutex_unlock(&ld->ops_lock);
 	return 0;
diff --git a/drivers/video/backlight/mbp_nvidia_bl.c b/drivers/video/backlight/mbp_nvidia_bl.c
new file mode 100644
index 00000000000..385cba40ea8
--- /dev/null
+++ b/drivers/video/backlight/mbp_nvidia_bl.c
@@ -0,0 +1,116 @@
+/*
+ *  Backlight Driver for Nvidia 8600 in Macbook Pro
+ *
+ *  Copyright (c) Red Hat <mjg@redhat.com>
+ *  Based on code from Pommed:
+ *  Copyright (C) 2006 Nicolas Boichat <nicolas @boichat.ch>
+ *  Copyright (C) 2006 Felipe Alfaro Solana <felipe_alfaro @linuxmail.org>
+ *  Copyright (C) 2007 Julien BLACHE <jb@jblache.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This driver triggers SMIs which cause the firmware to change the
+ *  backlight brightness. This is icky in many ways, but it's impractical to
+ *  get at the firmware code in order to figure out what it's actually doing.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/backlight.h>
+#include <linux/err.h>
+#include <linux/dmi.h>
+#include <linux/io.h>
+
+static struct backlight_device *mbp_backlight_device;
+
+static struct dmi_system_id __initdata mbp_device_table[] = {
+	{
+		.ident = "3,1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,1"),
+		},
+	},
+	{
+		.ident = "3,2",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,2"),
+		},
+	},
+	{
+		.ident = "4,1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro4,1"),
+		},
+	},
+	{ }
+};
+
+static int mbp_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+
+	outb(0x04 | (intensity << 4), 0xb3);
+	outb(0xbf, 0xb2);
+
+	return 0;
+}
+
+static int mbp_get_intensity(struct backlight_device *bd)
+{
+	outb(0x03, 0xb3);
+	outb(0xbf, 0xb2);
+	return inb(0xb3) >> 4;
+}
+
+static struct backlight_ops mbp_ops = {
+	.get_brightness = mbp_get_intensity,
+	.update_status  = mbp_send_intensity,
+};
+
+static int __init mbp_init(void)
+{
+	if (!dmi_check_system(mbp_device_table))
+		return -ENODEV;
+
+	if (!request_region(0xb2, 2, "Macbook Pro backlight"))
+		return -ENXIO;
+
+	mbp_backlight_device = backlight_device_register("mbp_backlight",
+							 NULL, NULL,
+							 &mbp_ops);
+	if (IS_ERR(mbp_backlight_device)) {
+		release_region(0xb2, 2);
+		return PTR_ERR(mbp_backlight_device);
+	}
+
+	mbp_backlight_device->props.max_brightness = 15;
+	mbp_backlight_device->props.brightness =
+		mbp_get_intensity(mbp_backlight_device);
+	backlight_update_status(mbp_backlight_device);
+
+	return 0;
+}
+
+static void __exit mbp_exit(void)
+{
+	backlight_device_unregister(mbp_backlight_device);
+
+	release_region(0xb2, 2);
+}
+
+module_init(mbp_init);
+module_exit(mbp_exit);
+
+MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
+MODULE_DESCRIPTION("Nvidia-based Macbook Pro Backlight Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("svnAppleInc.:pnMacBookPro3,1");
+MODULE_ALIAS("svnAppleInc.:pnMacBookPro3,2");
+MODULE_ALIAS("svnAppleInc.:pnMacBookPro4,1");
diff --git a/drivers/video/backlight/platform_lcd.c b/drivers/video/backlight/platform_lcd.c
new file mode 100644
index 00000000000..72d44dbfce8
--- /dev/null
+++ b/drivers/video/backlight/platform_lcd.c
@@ -0,0 +1,172 @@
+/* drivers/video/backlight/platform_lcd.c
+ *
+ * Copyright 2008 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>
+ *
+ * Generic platform-device LCD power control interface.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+*/
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/lcd.h>
+
+#include <video/platform_lcd.h>
+
+struct platform_lcd {
+	struct device		*us;
+	struct lcd_device	*lcd;
+	struct plat_lcd_data	*pdata;
+
+	unsigned int		 power;
+	unsigned int		 suspended : 1;
+};
+
+static inline struct platform_lcd *to_our_lcd(struct lcd_device *lcd)
+{
+	return lcd_get_data(lcd);
+}
+
+static int platform_lcd_get_power(struct lcd_device *lcd)
+{
+	struct platform_lcd *plcd = to_our_lcd(lcd);
+
+	return plcd->power;
+}
+
+static int platform_lcd_set_power(struct lcd_device *lcd, int power)
+{
+	struct platform_lcd *plcd = to_our_lcd(lcd);
+	int lcd_power = 1;
+
+	if (power == FB_BLANK_POWERDOWN || plcd->suspended)
+		lcd_power = 0;
+
+	plcd->pdata->set_power(plcd->pdata, lcd_power);
+	plcd->power = power;
+
+	return 0;
+}
+
+static int platform_lcd_match(struct lcd_device *lcd, struct fb_info *info)
+{
+	struct platform_lcd *plcd = to_our_lcd(lcd);
+	struct plat_lcd_data *pdata = plcd->pdata;
+
+	if (pdata->match_fb)
+		return pdata->match_fb(pdata, info);
+
+	return plcd->us->parent == info->device;
+}
+
+static struct lcd_ops platform_lcd_ops = {
+	.get_power	= platform_lcd_get_power,
+	.set_power	= platform_lcd_set_power,
+	.check_fb	= platform_lcd_match,
+};
+
+static int __devinit platform_lcd_probe(struct platform_device *pdev)
+{
+	struct plat_lcd_data *pdata;
+	struct platform_lcd *plcd;
+	struct device *dev = &pdev->dev;
+	int err;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(dev, "no platform data supplied\n");
+		return -EINVAL;
+	}
+
+	plcd = kzalloc(sizeof(struct platform_lcd), GFP_KERNEL);
+	if (!plcd) {
+		dev_err(dev, "no memory for state\n");
+		return -ENOMEM;
+	}
+
+	plcd->us = dev;
+	plcd->pdata = pdata;
+	plcd->lcd = lcd_device_register("platform-lcd", dev,
+					plcd, &platform_lcd_ops);
+	if (IS_ERR(plcd->lcd)) {
+		dev_err(dev, "cannot register lcd device\n");
+		err = PTR_ERR(plcd->lcd);
+		goto err_mem;
+	}
+
+	platform_set_drvdata(pdev, plcd);
+	return 0;
+
+ err_mem:
+	kfree(plcd);
+	return err;
+}
+
+static int __devexit platform_lcd_remove(struct platform_device *pdev)
+{
+	struct platform_lcd *plcd = platform_get_drvdata(pdev);
+
+	lcd_device_unregister(plcd->lcd);
+	kfree(plcd);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int platform_lcd_suspend(struct platform_device *pdev, pm_message_t st)
+{
+	struct platform_lcd *plcd = platform_get_drvdata(pdev);
+
+	plcd->suspended = 1;
+	platform_lcd_set_power(plcd->lcd, plcd->power);
+
+	return 0;
+}
+
+static int platform_lcd_resume(struct platform_device *pdev)
+{
+	struct platform_lcd *plcd = platform_get_drvdata(pdev);
+
+	plcd->suspended = 0;
+	platform_lcd_set_power(plcd->lcd, plcd->power);
+
+	return 0;
+}
+#else
+#define platform_lcd_suspend NULL
+#define platform_lcd_resume NULL
+#endif
+
+static struct platform_driver platform_lcd_driver = {
+	.driver		= {
+		.name	= "platform-lcd",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= platform_lcd_probe,
+	.remove		= __devexit_p(platform_lcd_remove),
+	.suspend        = platform_lcd_suspend,
+	.resume         = platform_lcd_resume,
+};
+
+static int __init platform_lcd_init(void)
+{
+	return platform_driver_register(&platform_lcd_driver);
+}
+
+static void __exit platform_lcd_cleanup(void)
+{
+	platform_driver_unregister(&platform_lcd_driver);
+}
+
+module_init(platform_lcd_init);
+module_exit(platform_lcd_cleanup);
+
+MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:platform-lcd");
diff --git a/drivers/video/backlight/vgg2432a4.c b/drivers/video/backlight/vgg2432a4.c
new file mode 100644
index 00000000000..593c7687d54
--- /dev/null
+++ b/drivers/video/backlight/vgg2432a4.c
@@ -0,0 +1,284 @@
+/* drivers/video/backlight/vgg2432a4.c
+ *
+ * VGG2432A4 (ILI9320) LCD controller driver.
+ *
+ * Copyright 2007 Simtec Electronics
+ *	http://armlinux.simtec.co.uk/
+ *	Ben Dooks <ben@simtec.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/delay.h>
+#include <linux/err.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/lcd.h>
+#include <linux/module.h>
+
+#include <linux/spi/spi.h>
+
+#include <video/ili9320.h>
+
+#include "ili9320.h"
+
+/* Device initialisation sequences */
+
+static struct ili9320_reg vgg_init1[] = {
+	{
+		.address = ILI9320_POWER1,
+		.value	 = ILI9320_POWER1_AP(0) | ILI9320_POWER1_BT(0),
+	}, {
+		.address = ILI9320_POWER2,
+		.value	 = (ILI9320_POWER2_VC(7) |
+			    ILI9320_POWER2_DC0(0) | ILI9320_POWER2_DC1(0)),
+	}, {
+		.address = ILI9320_POWER3,
+		.value	 = ILI9320_POWER3_VRH(0),
+	}, {
+		.address = ILI9320_POWER4,
+		.value	 = ILI9320_POWER4_VREOUT(0),
+	},
+};
+
+static struct ili9320_reg vgg_init2[] = {
+	{
+		.address = ILI9320_POWER1,
+		.value   = (ILI9320_POWER1_AP(3) | ILI9320_POWER1_APE |
+			    ILI9320_POWER1_BT(7) | ILI9320_POWER1_SAP),
+	}, {
+		.address = ILI9320_POWER2,
+		.value   = ILI9320_POWER2_VC(7) | ILI9320_POWER2_DC0(3),
+	}
+};
+
+static struct ili9320_reg vgg_gamma[] = {
+	{
+		.address = ILI9320_GAMMA1,
+		.value	 = 0x0000,
+	}, {
+		.address = ILI9320_GAMMA2,
+		.value   = 0x0505,
+	}, {
+		.address = ILI9320_GAMMA3,
+		.value	 = 0x0004,
+	}, {
+		.address = ILI9320_GAMMA4,
+		.value	 = 0x0006,
+	}, {
+		.address = ILI9320_GAMMA5,
+		.value	 = 0x0707,
+	}, {
+		.address = ILI9320_GAMMA6,
+		.value	 = 0x0105,
+	}, {
+		.address = ILI9320_GAMMA7,
+		.value	 = 0x0002,
+	}, {
+		.address = ILI9320_GAMMA8,
+		.value	 = 0x0707,
+	}, {
+		.address = ILI9320_GAMMA9,
+		.value	 = 0x0704,
+	}, {
+		.address = ILI9320_GAMMA10,
+		.value	 = 0x807,
+	}
+
+};
+
+static struct ili9320_reg vgg_init0[] = {
+	[0]	= {
+		/* set direction and scan mode gate */
+		.address = ILI9320_DRIVER,
+		.value	 = ILI9320_DRIVER_SS,
+	}, {
+		.address = ILI9320_DRIVEWAVE,
+		.value	 = (ILI9320_DRIVEWAVE_MUSTSET |
+			    ILI9320_DRIVEWAVE_EOR | ILI9320_DRIVEWAVE_BC),
+	}, {
+		.address = ILI9320_ENTRYMODE,
+		.value	 = ILI9320_ENTRYMODE_ID(3) | ILI9320_ENTRYMODE_BGR,
+	}, {
+		.address = ILI9320_RESIZING,
+		.value	 = 0x0,
+	},
+};
+
+
+static int vgg2432a4_lcd_init(struct ili9320 *lcd,
+			      struct ili9320_platdata *cfg)
+{
+	unsigned int addr;
+	int ret;
+
+	/* Set VCore before anything else (VGG243237-6UFLWA) */
+	ret = ili9320_write(lcd, 0x00e5, 0x8000);
+	if (ret)
+		goto err_initial;
+
+	/* Start the oscillator up before we can do anything else. */
+	ret = ili9320_write(lcd, ILI9320_OSCILATION, ILI9320_OSCILATION_OSC);
+	if (ret)
+		goto err_initial;
+
+	/* must wait at-lesat 10ms after starting */
+	mdelay(15);
+
+	ret = ili9320_write_regs(lcd, vgg_init0, ARRAY_SIZE(vgg_init0));
+	if (ret != 0)
+		goto err_initial;
+
+	ili9320_write(lcd, ILI9320_DISPLAY2, cfg->display2);
+	ili9320_write(lcd, ILI9320_DISPLAY3, cfg->display3);
+	ili9320_write(lcd, ILI9320_DISPLAY4, cfg->display4);
+
+	ili9320_write(lcd, ILI9320_RGB_IF1, cfg->rgb_if1);
+	ili9320_write(lcd, ILI9320_FRAMEMAKER, 0x0);
+	ili9320_write(lcd, ILI9320_RGB_IF2, ILI9320_RGBIF2_DPL);
+
+	ret = ili9320_write_regs(lcd, vgg_init1, ARRAY_SIZE(vgg_init1));
+	if (ret != 0)
+		goto err_vgg;
+
+	mdelay(300);
+
+	ret = ili9320_write_regs(lcd, vgg_init2, ARRAY_SIZE(vgg_init2));
+	if (ret != 0)
+		goto err_vgg2;
+
+	mdelay(100);
+
+	ili9320_write(lcd, ILI9320_POWER3, 0x13c);
+
+	mdelay(100);
+
+	ili9320_write(lcd, ILI9320_POWER4, 0x1c00);
+	ili9320_write(lcd, ILI9320_POWER7, 0x000e);
+
+	mdelay(100);
+
+	ili9320_write(lcd, ILI9320_GRAM_HORIZ_ADDR, 0x00);
+	ili9320_write(lcd, ILI9320_GRAM_VERT_ADD, 0x00);
+
+	ret = ili9320_write_regs(lcd, vgg_gamma, ARRAY_SIZE(vgg_gamma));
+	if (ret != 0)
+		goto err_vgg3;
+
+	ili9320_write(lcd, ILI9320_HORIZ_START, 0x0);
+	ili9320_write(lcd, ILI9320_HORIZ_END, cfg->hsize - 1);
+	ili9320_write(lcd, ILI9320_VERT_START, 0x0);
+	ili9320_write(lcd, ILI9320_VERT_END, cfg->vsize - 1);
+
+	ili9320_write(lcd, ILI9320_DRIVER2,
+		      ILI9320_DRIVER2_NL(((cfg->vsize - 240) / 8) + 0x1D));
+
+	ili9320_write(lcd, ILI9320_BASE_IMAGE, 0x1);
+	ili9320_write(lcd, ILI9320_VERT_SCROLL, 0x00);
+
+	for (addr = ILI9320_PARTIAL1_POSITION; addr <= ILI9320_PARTIAL2_END;
+	     addr++) {
+		ili9320_write(lcd, addr, 0x0);
+	}
+
+	ili9320_write(lcd, ILI9320_INTERFACE1, 0x10);
+	ili9320_write(lcd, ILI9320_INTERFACE2, cfg->interface2);
+	ili9320_write(lcd, ILI9320_INTERFACE3, cfg->interface3);
+	ili9320_write(lcd, ILI9320_INTERFACE4, cfg->interface4);
+	ili9320_write(lcd, ILI9320_INTERFACE5, cfg->interface5);
+	ili9320_write(lcd, ILI9320_INTERFACE6, cfg->interface6);
+
+	lcd->display1 = (ILI9320_DISPLAY1_D(3) | ILI9320_DISPLAY1_DTE |
+			 ILI9320_DISPLAY1_GON | ILI9320_DISPLAY1_BASEE |
+			 0x40);
+
+	ili9320_write(lcd, ILI9320_DISPLAY1, lcd->display1);
+
+	return 0;
+
+ err_vgg3:
+ err_vgg2:
+ err_vgg:
+ err_initial:
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int vgg2432a4_suspend(struct spi_device *spi, pm_message_t state)
+{
+	return ili9320_suspend(dev_get_drvdata(&spi->dev), state);
+}
+
+static int vgg2432a4_resume(struct spi_device *spi)
+{
+	return ili9320_resume(dev_get_drvdata(&spi->dev));
+}
+#else
+#define vgg2432a4_suspend	NULL
+#define vgg2432a4_resume 	NULL
+#endif
+
+static struct ili9320_client vgg2432a4_client = {
+	.name	= "VGG2432A4",
+	.init	= vgg2432a4_lcd_init,
+};
+
+/* Device probe */
+
+static int __devinit vgg2432a4_probe(struct spi_device *spi)
+{
+	int ret;
+
+	ret = ili9320_probe_spi(spi, &vgg2432a4_client);
+	if (ret != 0) {
+		dev_err(&spi->dev, "failed to initialise ili9320\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit vgg2432a4_remove(struct spi_device *spi)
+{
+	return ili9320_remove(dev_get_drvdata(&spi->dev));
+}
+
+static void vgg2432a4_shutdown(struct spi_device *spi)
+{
+	ili9320_shutdown(dev_get_drvdata(&spi->dev));
+}
+
+static struct spi_driver vgg2432a4_driver = {
+	.driver = {
+		.name		= "VGG2432A4",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= vgg2432a4_probe,
+	.remove		= __devexit_p(vgg2432a4_remove),
+	.shutdown	= vgg2432a4_shutdown,
+	.suspend	= vgg2432a4_suspend,
+	.resume		= vgg2432a4_resume,
+};
+
+/* Device driver initialisation */
+
+static int __init vgg2432a4_init(void)
+{
+	return spi_register_driver(&vgg2432a4_driver);
+}
+
+static void __exit vgg2432a4_exit(void)
+{
+	spi_unregister_driver(&vgg2432a4_driver);
+}
+
+module_init(vgg2432a4_init);
+module_exit(vgg2432a4_exit);
+
+MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
+MODULE_DESCRIPTION("VGG2432A4 LCD Driver");
+MODULE_LICENSE("GPL v2");
+
+
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index 49834a67a62..940467aed13 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -478,7 +478,7 @@ static int bfin_lcd_set_contrast(struct lcd_device *dev, int contrast)
 	return 0;
 }
 
-static int bfin_lcd_check_fb(struct fb_info *fi)
+static int bfin_lcd_check_fb(struct lcd_device *dev, struct fb_info *fi)
 {
 	if (!fi || (fi == &bfin_bf54x_fb))
 		return 1;
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 135d6dd7e67..7d1b819e501 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -396,7 +396,7 @@ static int bfin_lcd_set_contrast(struct lcd_device *dev, int contrast)
 	return 0;
 }
 
-static int bfin_lcd_check_fb(struct fb_info *fi)
+static int bfin_lcd_check_fb(struct lcd_device *dev, struct fb_info *fi)
 {
 	if (!fi || (fi == &bfin_t350mcqb_fb))
 		return 1;
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
new file mode 100644
index 00000000000..e15bb447440
--- /dev/null
+++ b/drivers/video/carminefb.c
@@ -0,0 +1,790 @@
+/*
+ * Frame buffer driver for the Carmine GPU.
+ *
+ * The driver configures the GPU as follows
+ * - FB0 is display 0 with unique memory area
+ * - FB1 is display 1 with unique memory area
+ * - both display use 32 bit colors
+ */
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+
+#include "carminefb.h"
+#include "carminefb_regs.h"
+
+#if !defined(__LITTLE_ENDIAN) && !defined(__BIG_ENDIAN)
+#error  "The endianness of the target host has not been defined."
+#endif
+
+/*
+ * The initial video mode can be supplied via two different ways:
+ * - as a string that is passed to fb_find_mode() (module option fb_mode_str)
+ * - as an integer that picks the video mode from carmine_modedb[] (module
+ *   option fb_mode)
+ *
+ * If nothing is used than the initial video mode will be the
+ * CARMINEFB_DEFAULT_VIDEO_MODE member of the carmine_modedb[].
+ */
+#define CARMINEFB_DEFAULT_VIDEO_MODE	1
+
+static unsigned int fb_mode = CARMINEFB_DEFAULT_VIDEO_MODE;
+module_param(fb_mode, uint, 444);
+MODULE_PARM_DESC(fb_mode, "Initial video mode as integer.");
+
+static char *fb_mode_str;
+module_param(fb_mode_str, charp, 444);
+MODULE_PARM_DESC(fb_mode_str, "Initial video mode in characters.");
+
+/*
+ * Carminefb displays:
+ * 0b000 None
+ * 0b001 Display 0
+ * 0b010 Display 1
+ */
+static int fb_displays = CARMINE_USE_DISPLAY0 | CARMINE_USE_DISPLAY1;
+module_param(fb_displays, int, 444);
+MODULE_PARM_DESC(fb_displays, "Bit mode, which displays are used");
+
+struct carmine_hw {
+	void __iomem *v_regs;
+	void __iomem *screen_mem;
+	struct fb_info *fb[MAX_DISPLAY];
+};
+
+struct carmine_resolution {
+	u32 htp;
+	u32 hsp;
+	u32 hsw;
+	u32 hdp;
+	u32 vtr;
+	u32 vsp;
+	u32 vsw;
+	u32 vdp;
+	u32 disp_mode;
+};
+
+struct carmine_fb {
+	void __iomem *display_reg;
+	void __iomem *screen_base;
+	u32 smem_offset;
+	u32 cur_mode;
+	u32 new_mode;
+	struct carmine_resolution *res;
+	u32 pseudo_palette[16];
+};
+
+static struct fb_fix_screeninfo carminefb_fix __devinitdata = {
+	.id = "Carmine",
+	.type = FB_TYPE_PACKED_PIXELS,
+	.visual = FB_VISUAL_TRUECOLOR,
+	.accel = FB_ACCEL_NONE,
+};
+
+static const struct fb_videomode carmine_modedb[] = {
+	{
+		.name		= "640x480",
+		.xres		= 640,
+		.yres		= 480,
+	}, {
+		.name		= "800x600",
+		.xres		= 800,
+		.yres		= 600,
+	},
+};
+
+static struct carmine_resolution car_modes[] = {
+	{
+		/* 640x480 */
+		.htp = 800,
+		.hsp = 672,
+		.hsw = 96,
+		.hdp = 640,
+		.vtr = 525,
+		.vsp = 490,
+		.vsw = 2,
+		.vdp = 480,
+		.disp_mode = 0x1400,
+	},
+	{
+		/* 800x600 */
+		.htp = 1060,
+		.hsp = 864,
+		.hsw = 72,
+		.hdp = 800,
+		.vtr = 628,
+		.vsp = 601,
+		.vsw = 2,
+		.vdp = 600,
+		.disp_mode = 0x0d00,
+	}
+};
+
+static int carmine_find_mode(const struct fb_var_screeninfo *var)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(car_modes); i++)
+		if (car_modes[i].hdp == var->xres &&
+		    car_modes[i].vdp == var->yres)
+			return i;
+	return -EINVAL;
+}
+
+static void c_set_disp_reg(const struct carmine_fb *par,
+		u32 offset, u32 val)
+{
+	writel(val, par->display_reg + offset);
+}
+
+static u32 c_get_disp_reg(const struct carmine_fb *par,
+		u32 offset)
+{
+	return readl(par->display_reg + offset);
+}
+
+static void c_set_hw_reg(const struct carmine_hw *hw,
+		u32 offset, u32 val)
+{
+	writel(val, hw->v_regs + offset);
+}
+
+static u32 c_get_hw_reg(const struct carmine_hw *hw,
+		u32 offset)
+{
+	return readl(hw->v_regs + offset);
+}
+
+static int carmine_setcolreg(unsigned regno, unsigned red, unsigned green,
+		unsigned blue, unsigned transp, struct fb_info *info)
+{
+	if (regno >= 16)
+		return 1;
+
+	red >>= 8;
+	green >>= 8;
+	blue >>= 8;
+	transp >>= 8;
+
+	((u32 *)info->pseudo_palette)[regno] = be32_to_cpu(transp << 24 |
+		red << 0 | green << 8 | blue << 16);
+	return 0;
+}
+
+static int carmine_check_var(struct fb_var_screeninfo *var,
+		struct fb_info *info)
+{
+	int ret;
+
+	ret = carmine_find_mode(var);
+	if (ret < 0)
+		return ret;
+
+	if (var->grayscale || var->rotate || var->nonstd)
+		return -EINVAL;
+
+	var->xres_virtual = var->xres;
+	var->yres_virtual = var->yres;
+
+	var->bits_per_pixel = 32;
+
+#ifdef __BIG_ENDIAN
+	var->transp.offset = 24;
+	var->red.offset = 0;
+	var->green.offset = 8;
+	var->blue.offset = 16;
+#else
+	var->transp.offset = 24;
+	var->red.offset = 16;
+	var->green.offset = 8;
+	var->blue.offset = 0;
+#endif
+
+	var->red.length = 8;
+	var->green.length = 8;
+	var->blue.length = 8;
+	var->transp.length = 8;
+
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.msb_right = 0;
+	return 0;
+}
+
+static void carmine_init_display_param(struct carmine_fb *par)
+{
+	u32 width;
+	u32 height;
+	u32 param;
+	u32 window_size;
+	u32 soffset = par->smem_offset;
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_C_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_MLMR_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_CURSOR_MODE,
+			CARMINE_CURSOR0_PRIORITY_MASK |
+			CARMINE_CURSOR1_PRIORITY_MASK |
+			CARMINE_CURSOR_CUTZ_MASK);
+
+	/* Set default cursor position */
+	c_set_disp_reg(par, CARMINE_DISP_REG_CUR1_POS, 0 << 16 | 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_CUR2_POS, 0 << 16 | 0);
+
+	/* Set default display mode */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_EXT_MODE, CARMINE_WINDOW_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_EXT_MODE,
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_EXT_MODE, CARMINE_EXTEND_MODE |
+			CARMINE_EXT_CMODE_DIRECT24_RGBA);
+
+	/* Set default frame size to layer mode register */
+	width = par->res->hdp * 4 / CARMINE_DISP_WIDTH_UNIT;
+	width = width << CARMINE_DISP_WIDTH_SHIFT;
+
+	height = par->res->vdp - 1;
+	param = width | height;
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_WIDTH, width);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_MODE_W_H, param);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_MODE_W_H, param);
+
+	/* Set default pos and size */
+	window_size = (par->res->vdp - 1) << CARMINE_DISP_WIN_H_SHIFT;
+	window_size |= par->res->hdp;
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_WIN_SIZE, window_size);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_WIN_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_WIN_SIZE, window_size);
+
+	/* Set default origin address */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_ORG_ADR, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_ORG_ADR, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_ORG_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_ORG_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_ORG_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_ORG_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_ORG_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_ORG_ADR1, soffset);
+
+	/* Set default display address */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_DISP_ADR, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_DISP_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_DISP_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_DISP_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_DISP_ADR1, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_DISP_ADR0, soffset);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_DISP_ADR0, soffset);
+
+	/* Set default display position */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_DISP_POS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_DISP_POS, 0);
+
+	/* Set default blend mode */
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L0, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L1, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L2, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L3, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L4, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L5, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L6, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_BLEND_MODE_L7, 0);
+
+	/* default transparency mode */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L1_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6_TRANS, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7_TRANS, 0);
+
+	/* Set default read skip parameter */
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6RM, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7RM, 0);
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6PX, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7PX, 0);
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_L0PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L2PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L3PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L4PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L5PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L6PY, 0);
+	c_set_disp_reg(par, CARMINE_DISP_REG_L7PY, 0);
+}
+
+static void set_display_parameters(struct carmine_fb *par)
+{
+	u32 mode;
+	u32 hdp, vdp, htp, hsp, hsw, vtr, vsp, vsw;
+
+	/*
+	 * display timing. Parameters are decreased by one because hardware
+	 * spec is 0 to (n - 1)
+	 * */
+	hdp = par->res->hdp - 1;
+	vdp = par->res->vdp - 1;
+	htp = par->res->htp - 1;
+	hsp = par->res->hsp - 1;
+	hsw = par->res->hsw - 1;
+	vtr = par->res->vtr - 1;
+	vsp = par->res->vsp - 1;
+	vsw = par->res->vsw - 1;
+
+	c_set_disp_reg(par, CARMINE_DISP_REG_H_TOTAL,
+			htp << CARMINE_DISP_HTP_SHIFT);
+	c_set_disp_reg(par, CARMINE_DISP_REG_H_PERIOD,
+			(hdp << CARMINE_DISP_HDB_SHIFT)	| hdp);
+	c_set_disp_reg(par, CARMINE_DISP_REG_V_H_W_H_POS,
+			(vsw << CARMINE_DISP_VSW_SHIFT) |
+			(hsw << CARMINE_DISP_HSW_SHIFT) |
+			(hsp));
+	c_set_disp_reg(par, CARMINE_DISP_REG_V_TOTAL,
+			vtr << CARMINE_DISP_VTR_SHIFT);
+	c_set_disp_reg(par, CARMINE_DISP_REG_V_PERIOD_POS,
+			(vdp << CARMINE_DISP_VDP_SHIFT) | vsp);
+
+	/* clock */
+	mode = c_get_disp_reg(par, CARMINE_DISP_REG_DCM1);
+	mode = (mode & ~CARMINE_DISP_DCM_MASK) |
+		(par->res->disp_mode & CARMINE_DISP_DCM_MASK);
+	/* enable video output and layer 0 */
+	mode |= CARMINE_DEN | CARMINE_L0E;
+	c_set_disp_reg(par, CARMINE_DISP_REG_DCM1, mode);
+}
+
+static int carmine_set_par(struct fb_info *info)
+{
+	struct carmine_fb *par = info->par;
+	int ret;
+
+	ret = carmine_find_mode(&info->var);
+	if (ret < 0)
+		return ret;
+
+	par->new_mode = ret;
+	if (par->cur_mode != par->new_mode) {
+
+		par->cur_mode = par->new_mode;
+		par->res = &car_modes[par->new_mode];
+
+		carmine_init_display_param(par);
+		set_display_parameters(par);
+	}
+
+	info->fix.line_length = info->var.xres * info->var.bits_per_pixel / 8;
+	return 0;
+}
+
+static int init_hardware(struct carmine_hw *hw)
+{
+	u32 flags;
+	u32 loops;
+	u32 ret;
+
+	/* Initalize Carmine */
+	/* Sets internal clock */
+	c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_CLOCK_ENABLE,
+			CARMINE_DFLT_IP_CLOCK_ENABLE);
+
+	/* Video signal output is turned off */
+	c_set_hw_reg(hw, CARMINE_DISP0_REG + CARMINE_DISP_REG_DCM1, 0);
+	c_set_hw_reg(hw, CARMINE_DISP1_REG + CARMINE_DISP_REG_DCM1, 0);
+
+	/* Software reset */
+	c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_SOFTWARE_RESET, 1);
+	c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_SOFTWARE_RESET, 0);
+
+	/* I/O mode settings */
+	flags = CARMINE_DFLT_IP_DCTL_IO_CONT1 << 16 |
+		CARMINE_DFLT_IP_DCTL_IO_CONT0;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_IOCONT1_IOCONT0,
+			flags);
+
+	/* DRAM initial sequence */
+	flags = CARMINE_DFLT_IP_DCTL_MODE << 16 | CARMINE_DFLT_IP_DCTL_ADD;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_MODE_ADD,
+			flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_SET_TIME1 << 16 |
+		CARMINE_DFLT_IP_DCTL_EMODE;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_SETTIME1_EMODE,
+			flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_REFRESH << 16 |
+		CARMINE_DFLT_IP_DCTL_SET_TIME2;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_REFRESH_SETTIME2,
+			flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_RESERVE2 << 16 |
+		CARMINE_DFLT_IP_DCTL_FIFO_DEPTH;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_RSV2_RSV1, flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_DDRIF2 << 16 | CARMINE_DFLT_IP_DCTL_DDRIF1;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_DDRIF2_DDRIF1,
+			flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_RESERVE0 << 16 |
+		CARMINE_DFLT_IP_DCTL_STATES;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_RSV0_STATES,
+			flags);
+
+	/* Executes DLL reset */
+	if (CARMINE_DCTL_DLL_RESET) {
+		for (loops = 0; loops < CARMINE_DCTL_INIT_WAIT_LIMIT; loops++) {
+
+			ret = c_get_hw_reg(hw, CARMINE_DCTL_REG +
+					CARMINE_DCTL_REG_RSV0_STATES);
+			ret &= CARMINE_DCTL_REG_STATES_MASK;
+			if (!ret)
+				break;
+
+			mdelay(CARMINE_DCTL_INIT_WAIT_INTERVAL);
+		}
+
+		if (loops >= CARMINE_DCTL_INIT_WAIT_LIMIT) {
+			printk(KERN_ERR "DRAM init failed\n");
+			return -EIO;
+		}
+	}
+
+	flags = CARMINE_DFLT_IP_DCTL_MODE_AFT_RST << 16 |
+		CARMINE_DFLT_IP_DCTL_ADD;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_MODE_ADD, flags);
+
+	flags = CARMINE_DFLT_IP_DCTL_RESERVE0 << 16 |
+		CARMINE_DFLT_IP_DCTL_STATES_AFT_RST;
+	c_set_hw_reg(hw, CARMINE_DCTL_REG + CARMINE_DCTL_REG_RSV0_STATES,
+			flags);
+
+	/* Initialize the write back register */
+	c_set_hw_reg(hw, CARMINE_WB_REG + CARMINE_WB_REG_WBM,
+			CARMINE_WB_REG_WBM_DEFAULT);
+
+	/* Initialize the Kottos registers */
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_VRINTM, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_VRERRM, 0);
+
+	/* Set DC offsets */
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_PX, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_PY, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_LX, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_LY, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_TX, 0);
+	c_set_hw_reg(hw, CARMINE_GRAPH_REG + CARMINE_GRAPH_REG_DC_OFFSET_TY, 0);
+	return 0;
+}
+
+static struct fb_ops carminefb_ops = {
+	.owner		= THIS_MODULE,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+
+	.fb_check_var	= carmine_check_var,
+	.fb_set_par	= carmine_set_par,
+	.fb_setcolreg	= carmine_setcolreg,
+};
+
+static int alloc_carmine_fb(void __iomem *regs, void __iomem *smem_base,
+		int smem_offset, struct device *device, struct fb_info **rinfo)
+{
+	int ret;
+	struct fb_info *info;
+	struct carmine_fb *par;
+
+	info = framebuffer_alloc(sizeof *par, device);
+	if (!info)
+		return -ENOMEM;
+
+	par = info->par;
+	par->display_reg = regs;
+	par->smem_offset = smem_offset;
+
+	info->screen_base = smem_base + smem_offset;
+	info->screen_size = CARMINE_DISPLAY_MEM;
+	info->fbops = &carminefb_ops;
+
+	info->fix = carminefb_fix;
+	info->pseudo_palette = par->pseudo_palette;
+	info->flags = FBINFO_DEFAULT;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 1);
+	if (ret < 0)
+		goto err_free_fb;
+
+	if (fb_mode > ARRAY_SIZE(carmine_modedb))
+		fb_mode = CARMINEFB_DEFAULT_VIDEO_MODE;
+
+	par->cur_mode = par->new_mode = ~0;
+
+	ret = fb_find_mode(&info->var, info, fb_mode_str, carmine_modedb,
+			ARRAY_SIZE(carmine_modedb),
+			&carmine_modedb[fb_mode], 32);
+	if (!ret || ret == 4) {
+		ret = -EINVAL;
+		goto err_dealloc_cmap;
+	}
+
+	fb_videomode_to_modelist(carmine_modedb, ARRAY_SIZE(carmine_modedb),
+			&info->modelist);
+
+	ret = register_framebuffer(info);
+	if (ret < 0)
+		goto err_dealloc_cmap;
+
+	printk(KERN_INFO "fb%d: %s frame buffer device\n", info->node,
+			info->fix.id);
+
+	*rinfo = info;
+	return 0;
+
+err_dealloc_cmap:
+	fb_dealloc_cmap(&info->cmap);
+err_free_fb:
+	framebuffer_release(info);
+	return ret;
+}
+
+static void cleanup_fb_device(struct fb_info *info)
+{
+	if (info) {
+		unregister_framebuffer(info);
+		fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+}
+
+static int __devinit carminefb_probe(struct pci_dev *dev,
+		const struct pci_device_id *ent)
+{
+	struct carmine_hw *hw;
+	struct device *device = &dev->dev;
+	struct fb_info *info;
+	int ret;
+
+	ret = pci_enable_device(dev);
+	if (ret)
+		return ret;
+
+	ret = -ENOMEM;
+	hw = kzalloc(sizeof *hw, GFP_KERNEL);
+	if (!hw)
+		goto err_enable_pci;
+
+	carminefb_fix.mmio_start = pci_resource_start(dev, CARMINE_CONFIG_BAR);
+	carminefb_fix.mmio_len = pci_resource_len(dev, CARMINE_CONFIG_BAR);
+
+	if (!request_mem_region(carminefb_fix.mmio_start,
+				carminefb_fix.mmio_len,
+				"carminefb regbase")) {
+		printk(KERN_ERR "carminefb: Can't reserve regbase.\n");
+		ret = -EBUSY;
+		goto err_free_hw;
+	}
+	hw->v_regs = ioremap_nocache(carminefb_fix.mmio_start,
+			carminefb_fix.mmio_len);
+	if (!hw->v_regs) {
+		printk(KERN_ERR "carminefb: Can't remap %s register.\n",
+				carminefb_fix.id);
+		goto err_free_reg_mmio;
+	}
+
+	carminefb_fix.smem_start = pci_resource_start(dev, CARMINE_MEMORY_BAR);
+	carminefb_fix.smem_len = pci_resource_len(dev, CARMINE_MEMORY_BAR);
+
+	/* The memory area tends to be very large (256 MiB). Remap only what
+	 * is required for that largest resolution to avoid remaps at run
+	 * time
+	 */
+	if (carminefb_fix.smem_len > CARMINE_TOTAL_DIPLAY_MEM)
+		carminefb_fix.smem_len = CARMINE_TOTAL_DIPLAY_MEM;
+
+	else if (carminefb_fix.smem_len < CARMINE_TOTAL_DIPLAY_MEM) {
+		printk(KERN_ERR "carminefb: Memory bar is only %d bytes, %d "
+				"are required.", carminefb_fix.smem_len,
+				CARMINE_TOTAL_DIPLAY_MEM);
+		goto err_free_reg_mmio;
+	}
+
+	if (!request_mem_region(carminefb_fix.smem_start,
+				carminefb_fix.smem_len,	"carminefb smem")) {
+		printk(KERN_ERR "carminefb: Can't reserve smem.\n");
+		goto err_unmap_vregs;
+	}
+
+	hw->screen_mem = ioremap_nocache(carminefb_fix.smem_start,
+			carminefb_fix.smem_len);
+	if (!hw->screen_mem) {
+		printk(KERN_ERR "carmine: Can't ioremap smem area.\n");
+		release_mem_region(carminefb_fix.smem_start,
+				carminefb_fix.smem_len);
+		goto err_reg_smem;
+	}
+
+	ret = init_hardware(hw);
+	if (ret)
+		goto err_unmap_screen;
+
+	info = NULL;
+	if (fb_displays & CARMINE_USE_DISPLAY0) {
+		ret = alloc_carmine_fb(hw->v_regs + CARMINE_DISP0_REG,
+				hw->screen_mem, CARMINE_DISPLAY_MEM * 0,
+				device, &info);
+		if (ret)
+			goto err_deinit_hw;
+	}
+
+	hw->fb[0] = info;
+
+	info = NULL;
+	if (fb_displays & CARMINE_USE_DISPLAY1) {
+		ret = alloc_carmine_fb(hw->v_regs + CARMINE_DISP1_REG,
+				hw->screen_mem, CARMINE_DISPLAY_MEM * 1,
+				device, &info);
+		if (ret)
+			goto err_cleanup_fb0;
+	}
+
+	hw->fb[1] = info;
+	info = NULL;
+
+	pci_set_drvdata(dev, hw);
+	return 0;
+
+err_cleanup_fb0:
+	cleanup_fb_device(hw->fb[0]);
+err_deinit_hw:
+	/* disable clock, etc */
+	c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_CLOCK_ENABLE, 0);
+err_unmap_screen:
+	iounmap(hw->screen_mem);
+err_reg_smem:
+	release_mem_region(carminefb_fix.mmio_start, carminefb_fix.mmio_len);
+err_unmap_vregs:
+	iounmap(hw->v_regs);
+err_free_reg_mmio:
+	release_mem_region(carminefb_fix.mmio_start, carminefb_fix.mmio_len);
+err_free_hw:
+	kfree(hw);
+err_enable_pci:
+	pci_disable_device(dev);
+	return ret;
+}
+
+static void __devexit carminefb_remove(struct pci_dev *dev)
+{
+	struct carmine_hw *hw = pci_get_drvdata(dev);
+	struct fb_fix_screeninfo fix;
+	int i;
+
+	/* in case we use only fb1 and not fb1 */
+	if (hw->fb[0])
+		fix = hw->fb[0]->fix;
+	else
+		fix = hw->fb[1]->fix;
+
+	/* deactivate display(s) and switch clocks */
+	c_set_hw_reg(hw, CARMINE_DISP0_REG + CARMINE_DISP_REG_DCM1, 0);
+	c_set_hw_reg(hw, CARMINE_DISP1_REG + CARMINE_DISP_REG_DCM1, 0);
+	c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_CLOCK_ENABLE, 0);
+
+	for (i = 0; i < MAX_DISPLAY; i++)
+		cleanup_fb_device(hw->fb[i]);
+
+	iounmap(hw->screen_mem);
+	release_mem_region(fix.smem_start, fix.smem_len);
+	iounmap(hw->v_regs);
+	release_mem_region(fix.mmio_start, fix.mmio_len);
+
+	pci_set_drvdata(dev, NULL);
+	pci_disable_device(dev);
+	kfree(hw);
+}
+
+#define PCI_VENDOR_ID_FUJITU_LIMITED 0x10cf
+static struct pci_device_id carmine_devices[] __devinitdata = {
+{
+	PCI_DEVICE(PCI_VENDOR_ID_FUJITU_LIMITED, 0x202b)},
+	{0, 0, 0, 0, 0, 0, 0}
+};
+
+MODULE_DEVICE_TABLE(pci, carmine_devices);
+
+static struct pci_driver carmine_pci_driver = {
+	.name		= "carminefb",
+	.id_table	= carmine_devices,
+	.probe		= carminefb_probe,
+	.remove		= __devexit_p(carminefb_remove),
+};
+
+static int __init carminefb_init(void)
+{
+	if (!(fb_displays &
+		(CARMINE_USE_DISPLAY0 | CARMINE_USE_DISPLAY1))) {
+		printk(KERN_ERR "If you disable both displays than you don't "
+				"need the driver at all\n");
+		return -EINVAL;
+	}
+	return pci_register_driver(&carmine_pci_driver);
+}
+module_init(carminefb_init);
+
+static void __exit carminefb_cleanup(void)
+{
+	pci_unregister_driver(&carmine_pci_driver);
+}
+module_exit(carminefb_cleanup);
+
+MODULE_AUTHOR("Sebastian Siewior <bigeasy@linutronix.de>");
+MODULE_DESCRIPTION("Framebuffer driver for Fujitsu Carmine based devices");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/carminefb.h b/drivers/video/carminefb.h
new file mode 100644
index 00000000000..05306de0c6b
--- /dev/null
+++ b/drivers/video/carminefb.h
@@ -0,0 +1,64 @@
+#ifndef CARMINE_CARMINE_H
+#define CARMINE_CARMINE_H
+
+#define CARMINE_MEMORY_BAR	2
+#define CARMINE_CONFIG_BAR	3
+
+#define MAX_DISPLAY	2
+#define CARMINE_DISPLAY_MEM	(800 * 600 * 4)
+#define CARMINE_TOTAL_DIPLAY_MEM	(CARMINE_DISPLAY_MEM * MAX_DISPLAY)
+
+#define CARMINE_USE_DISPLAY0	(1 << 0)
+#define CARMINE_USE_DISPLAY1	(1 << 1)
+
+/*
+ * This values work on the eval card. Custom boards may use different timings,
+ * here an example :)
+ */
+
+/* DRAM initialization values */
+#ifdef CONFIG_FB_CARMINE_DRAM_EVAL
+
+#define CARMINE_DFLT_IP_CLOCK_ENABLE		(0x03ff)
+#define CARMINE_DFLT_IP_DCTL_ADD		(0x05c3)
+#define CARMINE_DFLT_IP_DCTL_MODE		(0x0121)
+#define CARMINE_DFLT_IP_DCTL_EMODE		(0x8000)
+#define CARMINE_DFLT_IP_DCTL_SET_TIME1		(0x4749)
+#define CARMINE_DFLT_IP_DCTL_SET_TIME2		(0x2a22)
+#define CARMINE_DFLT_IP_DCTL_REFRESH		(0x0042)
+#define CARMINE_DFLT_IP_DCTL_STATES		(0x0003)
+#define CARMINE_DFLT_IP_DCTL_RESERVE0		(0x0020)
+#define CARMINE_DFLT_IP_DCTL_FIFO_DEPTH		(0x000f)
+#define CARMINE_DFLT_IP_DCTL_RESERVE2		(0x0000)
+#define CARMINE_DFLT_IP_DCTL_DDRIF1		(0x6646)
+#define CARMINE_DFLT_IP_DCTL_DDRIF2		(0x0055)
+#define CARMINE_DFLT_IP_DCTL_MODE_AFT_RST	(0x0021)
+#define CARMINE_DFLT_IP_DCTL_STATES_AFT_RST	(0x0002)
+#define CARMINE_DFLT_IP_DCTL_IO_CONT0		(0x0555)
+#define CARMINE_DFLT_IP_DCTL_IO_CONT1		(0x0555)
+#define CARMINE_DCTL_DLL_RESET			(1)
+#endif
+
+#ifdef CONFIG_CARMINE_DRAM_CUSTOM
+
+#define CARMINE_DFLT_IP_CLOCK_ENABLE		(0x03ff)
+#define CARMINE_DFLT_IP_DCTL_ADD		(0x03b2)
+#define CARMINE_DFLT_IP_DCTL_MODE		(0x0161)
+#define CARMINE_DFLT_IP_DCTL_EMODE		(0x8000)
+#define CARMINE_DFLT_IP_DCTL_SET_TIME1		(0x2628)
+#define CARMINE_DFLT_IP_DCTL_SET_TIME2		(0x1a09)
+#define CARMINE_DFLT_IP_DCTL_REFRESH		(0x00fe)
+#define CARMINE_DFLT_IP_DCTL_STATES		(0x0003)
+#define CARMINE_DFLT_IP_DCTL_RESERVE0		(0x0020)
+#define CARMINE_DFLT_IP_DCTL_FIFO_DEPTH		(0x000f)
+#define CARMINE_DFLT_IP_DCTL_RESERVE2		(0x0000)
+#define CARMINE_DFLT_IP_DCTL_DDRIF1		(0x0646)
+#define CARMINE_DFLT_IP_DCTL_DDRIF2		(0x55aa)
+#define CARMINE_DFLT_IP_DCTL_MODE_AFT_RST	(0x0061)
+#define CARMINE_DFLT_IP_DCTL_STATES_AFT_RST	(0x0002)
+#define CARMINE_DFLT_IP_DCTL_IO_CONT0		(0x0555)
+#define CARMINE_DFLT_IP_DCTL_IO_CONT1		(0x0555)
+#define CARMINE_DCTL_DLL_RESET			(1)
+#endif
+
+#endif
diff --git a/drivers/video/carminefb_regs.h b/drivers/video/carminefb_regs.h
new file mode 100644
index 00000000000..045215600b7
--- /dev/null
+++ b/drivers/video/carminefb_regs.h
@@ -0,0 +1,159 @@
+#ifndef _CARMINEFB_REGS_H
+#define _CARMINEFB_REGS_H
+
+#define CARMINE_OVERLAY_EXT_MODE	(0x00000002)
+#define CARMINE_GRAPH_REG		(0x00000000)
+#define CARMINE_DISP0_REG		(0x00100000)
+#define CARMINE_DISP1_REG		(0x00140000)
+#define CARMINE_WB_REG			(0x00180000)
+#define CARMINE_DCTL_REG		(0x00300000)
+#define CARMINE_CTL_REG			(0x00400000)
+#define CARMINE_WINDOW_MODE		(0x00000001)
+#define CARMINE_EXTEND_MODE		(CARMINE_WINDOW_MODE | \
+					CARMINE_OVERLAY_EXT_MODE)
+#define CARMINE_L0E			(1 << 16)
+#define CARMINE_L2E			(1 << 18)
+#define CARMINE_DEN			(1 << 31)
+
+#define CARMINE_EXT_CMODE_DIRECT24_RGBA		(0xC0000000)
+#define CARMINE_DCTL_REG_MODE_ADD		(0x00)
+#define CARMINE_DCTL_REG_SETTIME1_EMODE		(0x04)
+#define CARMINE_DCTL_REG_REFRESH_SETTIME2	(0x08)
+#define CARMINE_DCTL_REG_RSV0_STATES		(0x0C)
+#define CARMINE_DCTL_REG_RSV2_RSV1		(0x10)
+#define CARMINE_DCTL_REG_DDRIF2_DDRIF1		(0x14)
+#define CARMINE_DCTL_REG_IOCONT1_IOCONT0	(0x24)
+#define CARMINE_DCTL_REG_STATES_MASK		(0x000F)
+#define CARMINE_DCTL_INIT_WAIT_INTERVAL		(1)
+#define CARMINE_DCTL_INIT_WAIT_LIMIT		(5000)
+#define CARMINE_WB_REG_WBM_DEFAULT		(0x0001c020)
+#define CARMINE_DISP_REG_L0RM			(0x1880)
+#define CARMINE_DISP_REG_L0PX			(0x1884)
+#define CARMINE_DISP_REG_L0PY			(0x1888)
+#define CARMINE_DISP_REG_L2RM			(0x18A0)
+#define CARMINE_DISP_REG_L2PX			(0x18A4)
+#define CARMINE_DISP_REG_L2PY			(0x18A8)
+#define CARMINE_DISP_REG_L3RM			(0x18B0)
+#define CARMINE_DISP_REG_L3PX			(0x18B4)
+#define CARMINE_DISP_REG_L3PY			(0x18B8)
+#define CARMINE_DISP_REG_L4RM			(0x18C0)
+#define CARMINE_DISP_REG_L4PX			(0x18C4)
+#define CARMINE_DISP_REG_L4PY			(0x18C8)
+#define CARMINE_DISP_REG_L5RM			(0x18D0)
+#define CARMINE_DISP_REG_L5PX			(0x18D4)
+#define CARMINE_DISP_REG_L5PY			(0x18D8)
+#define CARMINE_DISP_REG_L6RM			(0x1924)
+#define CARMINE_DISP_REG_L6PX			(0x1928)
+#define CARMINE_DISP_REG_L6PY			(0x192C)
+#define CARMINE_DISP_REG_L7RM			(0x1964)
+#define CARMINE_DISP_REG_L7PX			(0x1968)
+#define CARMINE_DISP_REG_L7PY			(0x196C)
+#define CARMINE_WB_REG_WBM			(0x0004)
+#define CARMINE_DISP_HTP_SHIFT			(16)
+#define CARMINE_DISP_HDB_SHIFT			(16)
+#define CARMINE_DISP_HSW_SHIFT			(16)
+#define CARMINE_DISP_VSW_SHIFT			(24)
+#define CARMINE_DISP_VTR_SHIFT			(16)
+#define CARMINE_DISP_VDP_SHIFT			(16)
+#define CARMINE_CURSOR_CUTZ_MASK		(0x00000100)
+#define CARMINE_CURSOR0_PRIORITY_MASK		(0x00010000)
+#define CARMINE_CURSOR1_PRIORITY_MASK		(0x00020000)
+#define CARMINE_DISP_WIDTH_SHIFT		(16)
+#define CARMINE_DISP_WIN_H_SHIFT		(16)
+#define CARMINE_DISP_REG_H_TOTAL		(0x0004)
+#define CARMINE_DISP_REG_H_PERIOD		(0x0008)
+#define CARMINE_DISP_REG_V_H_W_H_POS		(0x000C)
+#define CARMINE_DISP_REG_V_TOTAL		(0x0010)
+#define CARMINE_DISP_REG_V_PERIOD_POS		(0x0014)
+#define CARMINE_DISP_REG_L0_MODE_W_H		(0x0020)
+#define CARMINE_DISP_REG_L0_ORG_ADR		(0x0024)
+#define CARMINE_DISP_REG_L0_DISP_ADR		(0x0028)
+#define CARMINE_DISP_REG_L0_DISP_POS		(0x002C)
+#define CARMINE_DISP_REG_L1_WIDTH		(0x0030)
+#define CARMINE_DISP_REG_L1_ORG_ADR		(0x0034)
+#define CARMINE_DISP_REG_L2_MODE_W_H		(0x0040)
+#define CARMINE_DISP_REG_L2_ORG_ADR1		(0x0044)
+#define CARMINE_DISP_REG_L2_DISP_ADR1		(0x0048)
+#define CARMINE_DISP_REG_L2_DISP_POS		(0x0054)
+#define CARMINE_DISP_REG_L3_MODE_W_H		(0x0058)
+#define CARMINE_DISP_REG_L3_ORG_ADR1		(0x005C)
+#define CARMINE_DISP_REG_L3_DISP_ADR1		(0x0060)
+#define CARMINE_DISP_REG_L3_DISP_POS		(0x006C)
+#define CARMINE_DISP_REG_L4_MODE_W_H		(0x0070)
+#define CARMINE_DISP_REG_L4_ORG_ADR1		(0x0074)
+#define CARMINE_DISP_REG_L4_DISP_ADR1		(0x0078)
+#define CARMINE_DISP_REG_L4_DISP_POS		(0x0084)
+#define CARMINE_DISP_REG_L5_MODE_W_H		(0x0088)
+#define CARMINE_DISP_REG_L5_ORG_ADR1		(0x008C)
+#define CARMINE_DISP_REG_L5_DISP_ADR1		(0x0090)
+#define CARMINE_DISP_REG_L5_DISP_POS		(0x009C)
+#define CARMINE_DISP_REG_CURSOR_MODE		(0x00A0)
+#define CARMINE_DISP_REG_CUR1_POS		(0x00A8)
+#define CARMINE_DISP_REG_CUR2_POS		(0x00B0)
+#define CARMINE_DISP_REG_C_TRANS		(0x00BC)
+#define CARMINE_DISP_REG_MLMR_TRANS		(0x00C0)
+#define CARMINE_DISP_REG_L0_EXT_MODE		(0x0110)
+#define CARMINE_DISP_REG_L0_WIN_POS		(0x0114)
+#define CARMINE_DISP_REG_L0_WIN_SIZE		(0x0118)
+#define CARMINE_DISP_REG_L1_EXT_MODE		(0x0120)
+#define CARMINE_DISP_REG_L1_WIN_POS		(0x0124)
+#define CARMINE_DISP_REG_L1_WIN_SIZE		(0x0128)
+#define CARMINE_DISP_REG_L2_EXT_MODE		(0x0130)
+#define CARMINE_DISP_REG_L2_WIN_POS		(0x0134)
+#define CARMINE_DISP_REG_L2_WIN_SIZE		(0x0138)
+#define CARMINE_DISP_REG_L3_EXT_MODE		(0x0140)
+#define CARMINE_DISP_REG_L3_WIN_POS		(0x0144)
+#define CARMINE_DISP_REG_L3_WIN_SIZE		(0x0148)
+#define CARMINE_DISP_REG_L4_EXT_MODE		(0x0150)
+#define CARMINE_DISP_REG_L4_WIN_POS		(0x0154)
+#define CARMINE_DISP_REG_L4_WIN_SIZE		(0x0158)
+#define CARMINE_DISP_REG_L5_EXT_MODE		(0x0160)
+#define CARMINE_DISP_REG_L5_WIN_POS		(0x0164)
+#define CARMINE_DISP_REG_L5_WIN_SIZE		(0x0168)
+#define CARMINE_DISP_REG_L6_EXT_MODE		(0x1918)
+#define CARMINE_DISP_REG_L6_WIN_POS		(0x191c)
+#define CARMINE_DISP_REG_L6_WIN_SIZE		(0x1920)
+#define CARMINE_DISP_REG_L7_EXT_MODE		(0x1958)
+#define CARMINE_DISP_REG_L7_WIN_POS		(0x195c)
+#define CARMINE_DISP_REG_L7_WIN_SIZE		(0x1960)
+#define CARMINE_DISP_REG_BLEND_MODE_L0		(0x00B4)
+#define CARMINE_DISP_REG_BLEND_MODE_L1		(0x0188)
+#define CARMINE_DISP_REG_BLEND_MODE_L2		(0x018C)
+#define CARMINE_DISP_REG_BLEND_MODE_L3		(0x0190)
+#define CARMINE_DISP_REG_BLEND_MODE_L4		(0x0194)
+#define CARMINE_DISP_REG_BLEND_MODE_L5		(0x0198)
+#define CARMINE_DISP_REG_BLEND_MODE_L6		(0x1990)
+#define CARMINE_DISP_REG_BLEND_MODE_L7		(0x1994)
+#define CARMINE_DISP_REG_L0_TRANS		(0x01A0)
+#define CARMINE_DISP_REG_L1_TRANS		(0x01A4)
+#define CARMINE_DISP_REG_L2_TRANS		(0x01A8)
+#define CARMINE_DISP_REG_L3_TRANS		(0x01AC)
+#define CARMINE_DISP_REG_L4_TRANS		(0x01B0)
+#define CARMINE_DISP_REG_L5_TRANS		(0x01B4)
+#define CARMINE_DISP_REG_L6_TRANS		(0x1998)
+#define CARMINE_DISP_REG_L7_TRANS		(0x199c)
+#define CARMINE_EXTEND_MODE_MASK		(0x00000003)
+#define CARMINE_DISP_DCM_MASK			(0x0000FFFF)
+#define CARMINE_DISP_REG_DCM1			(0x0100)
+#define CARMINE_DISP_WIDTH_UNIT			(64)
+#define CARMINE_DISP_REG_L6_MODE_W_H		(0x1900)
+#define CARMINE_DISP_REG_L6_ORG_ADR1		(0x1904)
+#define CARMINE_DISP_REG_L6_DISP_ADR0		(0x1908)
+#define CARMINE_DISP_REG_L6_DISP_POS		(0x1914)
+#define CARMINE_DISP_REG_L7_MODE_W_H		(0x1940)
+#define CARMINE_DISP_REG_L7_ORG_ADR1		(0x1944)
+#define CARMINE_DISP_REG_L7_DISP_ADR0		(0x1948)
+#define CARMINE_DISP_REG_L7_DISP_POS		(0x1954)
+#define CARMINE_CTL_REG_CLOCK_ENABLE		(0x000C)
+#define CARMINE_CTL_REG_SOFTWARE_RESET		(0x0010)
+#define CARMINE_CTL_REG_IST_MASK_ALL		(0x07FFFFFF)
+#define CARMINE_GRAPH_REG_VRINTM		(0x00028064)
+#define CARMINE_GRAPH_REG_VRERRM		(0x0002806C)
+#define CARMINE_GRAPH_REG_DC_OFFSET_PX		(0x0004005C)
+#define CARMINE_GRAPH_REG_DC_OFFSET_PY		(0x00040060)
+#define CARMINE_GRAPH_REG_DC_OFFSET_LX		(0x00040064)
+#define CARMINE_GRAPH_REG_DC_OFFSET_LY		(0x00040068)
+#define CARMINE_GRAPH_REG_DC_OFFSET_TX		(0x0004006C)
+#define CARMINE_GRAPH_REG_DC_OFFSET_TY		(0x00040070)
+
+#endif
diff --git a/drivers/video/cobalt_lcdfb.c b/drivers/video/cobalt_lcdfb.c
new file mode 100644
index 00000000000..7bad24ed04e
--- /dev/null
+++ b/drivers/video/cobalt_lcdfb.c
@@ -0,0 +1,371 @@
+/*
+ *  Cobalt server LCD frame buffer driver.
+ *
+ *  Copyright (C) 2008  Yoichi Yuasa <yoichi_yuasa@tripeaks.co.jp>
+ *
+ *  This program is free software; 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/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+
+/*
+ * Cursor position address
+ * \X  0    1    2  ...  14   15
+ * Y+----+----+----+---+----+----+
+ * 0|0x00|0x01|0x02|...|0x0e|0x0f|
+ *  +----+----+----+---+----+----+
+ * 1|0x40|0x41|0x42|...|0x4e|0x4f|
+ *  +----+----+----+---+----+----+
+ */
+#define LCD_DATA_REG_OFFSET	0x10
+#define LCD_XRES_MAX		16
+#define LCD_YRES_MAX		2
+#define LCD_CHARS_MAX		32
+
+#define LCD_CLEAR		0x01
+#define LCD_CURSOR_MOVE_HOME	0x02
+#define LCD_RESET		0x06
+#define LCD_OFF			0x08
+#define LCD_CURSOR_OFF		0x0c
+#define LCD_CURSOR_BLINK_OFF	0x0e
+#define LCD_CURSOR_ON		0x0f
+#define LCD_ON			LCD_CURSOR_ON
+#define LCD_CURSOR_MOVE_LEFT	0x10
+#define LCD_CURSOR_MOVE_RIGHT	0x14
+#define LCD_DISPLAY_LEFT	0x18
+#define LCD_DISPLAY_RIGHT	0x1c
+#define LCD_PRERESET		0x3f	/* execute 4 times continuously */
+#define LCD_BUSY		0x80
+
+#define LCD_GRAPHIC_MODE	0x40
+#define LCD_TEXT_MODE		0x80
+#define LCD_CUR_POS_MASK	0x7f
+
+#define LCD_CUR_POS(x)		((x) & LCD_CUR_POS_MASK)
+#define LCD_TEXT_POS(x)		((x) | LCD_TEXT_MODE)
+
+static inline void lcd_write_control(struct fb_info *info, u8 control)
+{
+	writel((u32)control << 24, info->screen_base);
+}
+
+static inline u8 lcd_read_control(struct fb_info *info)
+{
+	return readl(info->screen_base) >> 24;
+}
+
+static inline void lcd_write_data(struct fb_info *info, u8 data)
+{
+	writel((u32)data << 24, info->screen_base + LCD_DATA_REG_OFFSET);
+}
+
+static inline u8 lcd_read_data(struct fb_info *info)
+{
+	return readl(info->screen_base + LCD_DATA_REG_OFFSET) >> 24;
+}
+
+static int lcd_busy_wait(struct fb_info *info)
+{
+	u8 val = 0;
+	int timeout = 10, retval = 0;
+
+	do {
+		val = lcd_read_control(info);
+		val &= LCD_BUSY;
+		if (val != LCD_BUSY)
+			break;
+
+		if (msleep_interruptible(1))
+			return -EINTR;
+
+		timeout--;
+	} while (timeout);
+
+	if (val == LCD_BUSY)
+		retval = -EBUSY;
+
+	return retval;
+}
+
+static void lcd_clear(struct fb_info *info)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		udelay(150);
+
+		lcd_write_control(info, LCD_PRERESET);
+	}
+
+	udelay(150);
+
+	lcd_write_control(info, LCD_CLEAR);
+
+	udelay(150);
+
+	lcd_write_control(info, LCD_RESET);
+}
+
+static struct fb_fix_screeninfo cobalt_lcdfb_fix __initdata = {
+	.id		= "cobalt-lcd",
+	.type		= FB_TYPE_TEXT,
+	.type_aux	= FB_AUX_TEXT_MDA,
+	.visual		= FB_VISUAL_MONO01,
+	.line_length	= LCD_XRES_MAX,
+	.accel		= FB_ACCEL_NONE,
+};
+
+static ssize_t cobalt_lcdfb_read(struct fb_info *info, char __user *buf,
+				 size_t count, loff_t *ppos)
+{
+	char src[LCD_CHARS_MAX];
+	unsigned long pos;
+	int len, retval = 0;
+
+	pos = *ppos;
+	if (pos >= LCD_CHARS_MAX || count == 0)
+		return 0;
+
+	if (count > LCD_CHARS_MAX)
+		count = LCD_CHARS_MAX;
+
+	if (pos + count > LCD_CHARS_MAX)
+		count = LCD_CHARS_MAX - pos;
+
+	for (len = 0; len < count; len++) {
+		retval = lcd_busy_wait(info);
+		if (retval < 0)
+			break;
+
+		lcd_write_control(info, LCD_TEXT_POS(pos));
+
+		retval = lcd_busy_wait(info);
+		if (retval < 0)
+			break;
+
+		src[len] = lcd_read_data(info);
+		if (pos == 0x0f)
+			pos = 0x40;
+		else
+			pos++;
+	}
+
+	if (retval < 0 && signal_pending(current))
+		return -ERESTARTSYS;
+
+	if (copy_to_user(buf, src, len))
+		return -EFAULT;
+
+	*ppos += len;
+
+	return len;
+}
+
+static ssize_t cobalt_lcdfb_write(struct fb_info *info, const char __user *buf,
+				  size_t count, loff_t *ppos)
+{
+	char dst[LCD_CHARS_MAX];
+	unsigned long pos;
+	int len, retval = 0;
+
+	pos = *ppos;
+	if (pos >= LCD_CHARS_MAX || count == 0)
+		return 0;
+
+	if (count > LCD_CHARS_MAX)
+		count = LCD_CHARS_MAX;
+
+	if (pos + count > LCD_CHARS_MAX)
+		count = LCD_CHARS_MAX - pos;
+
+	if (copy_from_user(dst, buf, count))
+		return -EFAULT;
+
+	for (len = 0; len < count; len++) {
+		retval = lcd_busy_wait(info);
+		if (retval < 0)
+			break;
+
+		lcd_write_control(info, LCD_TEXT_POS(pos));
+
+		retval = lcd_busy_wait(info);
+		if (retval < 0)
+			break;
+
+		lcd_write_data(info, dst[len]);
+		if (pos == 0x0f)
+			pos = 0x40;
+		else
+			pos++;
+	}
+
+	if (retval < 0 && signal_pending(current))
+		return -ERESTARTSYS;
+
+	*ppos += len;
+
+	return len;
+}
+
+static int cobalt_lcdfb_blank(int blank_mode, struct fb_info *info)
+{
+	int retval;
+
+	retval = lcd_busy_wait(info);
+	if (retval < 0)
+		return retval;
+
+	switch (blank_mode) {
+	case FB_BLANK_UNBLANK:
+		lcd_write_control(info, LCD_ON);
+		break;
+	default:
+		lcd_write_control(info, LCD_OFF);
+		break;
+	}
+
+	return 0;
+}
+
+static int cobalt_lcdfb_cursor(struct fb_info *info, struct fb_cursor *cursor)
+{
+	u32 x, y;
+	int retval;
+
+	switch (cursor->set) {
+	case FB_CUR_SETPOS:
+		x = cursor->image.dx;
+		y = cursor->image.dy;
+		if (x >= LCD_XRES_MAX || y >= LCD_YRES_MAX)
+			return -EINVAL;
+
+		retval = lcd_busy_wait(info);
+		if (retval < 0)
+			return retval;
+
+		lcd_write_control(info,
+				  LCD_TEXT_POS(info->fix.line_length * y + x));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	retval = lcd_busy_wait(info);
+	if (retval < 0)
+		return retval;
+
+	if (cursor->enable)
+		lcd_write_control(info, LCD_CURSOR_ON);
+	else
+		lcd_write_control(info, LCD_CURSOR_OFF);
+
+	return 0;
+}
+
+static struct fb_ops cobalt_lcd_fbops = {
+	.owner		= THIS_MODULE,
+	.fb_read	= cobalt_lcdfb_read,
+	.fb_write	= cobalt_lcdfb_write,
+	.fb_blank	= cobalt_lcdfb_blank,
+	.fb_cursor	= cobalt_lcdfb_cursor,
+};
+
+static int __init cobalt_lcdfb_probe(struct platform_device *dev)
+{
+	struct fb_info *info;
+	struct resource *res;
+	int retval;
+
+	info = framebuffer_alloc(0, &dev->dev);
+	if (!info)
+		return -ENOMEM;
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res) {
+		framebuffer_release(info);
+		return -EBUSY;
+	}
+
+	info->screen_size = res->end - res->start + 1;
+	info->screen_base = ioremap(res->start, info->screen_size);
+	info->fbops = &cobalt_lcd_fbops;
+	info->fix = cobalt_lcdfb_fix;
+	info->fix.smem_start = res->start;
+	info->fix.smem_len = info->screen_size;
+	info->pseudo_palette = NULL;
+	info->par = NULL;
+	info->flags = FBINFO_DEFAULT;
+
+	retval = register_framebuffer(info);
+	if (retval < 0) {
+		iounmap(info->screen_base);
+		framebuffer_release(info);
+		return retval;
+	}
+
+	platform_set_drvdata(dev, info);
+
+	lcd_clear(info);
+
+	printk(KERN_INFO "fb%d: Cobalt server LCD frame buffer device\n",
+		info->node);
+
+	return 0;
+}
+
+static int __devexit cobalt_lcdfb_remove(struct platform_device *dev)
+{
+	struct fb_info *info;
+
+	info = platform_get_drvdata(dev);
+	if (info) {
+		iounmap(info->screen_base);
+		unregister_framebuffer(info);
+		framebuffer_release(info);
+	}
+
+	return 0;
+}
+
+static struct platform_driver cobalt_lcdfb_driver = {
+	.probe	= cobalt_lcdfb_probe,
+	.remove	= __devexit_p(cobalt_lcdfb_remove),
+	.driver	= {
+		.name	= "cobalt-lcd",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init cobalt_lcdfb_init(void)
+{
+	return platform_driver_register(&cobalt_lcdfb_driver);
+}
+
+static void __exit cobalt_lcdfb_exit(void)
+{
+	platform_driver_unregister(&cobalt_lcdfb_driver);
+}
+
+module_init(cobalt_lcdfb_init);
+module_exit(cobalt_lcdfb_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Yoichi Yuasa");
+MODULE_DESCRIPTION("Cobalt server LCD frame buffer driver");
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 4be3b46c069..3ccfa76d9b2 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -107,9 +107,7 @@ static struct display fb_display[MAX_NR_CONSOLES];
 
 static signed char con2fb_map[MAX_NR_CONSOLES];
 static signed char con2fb_map_boot[MAX_NR_CONSOLES];
-#ifndef MODULE
-static int logo_height;
-#endif
+
 static int logo_lines;
 /* logo_shown is an index to vc_cons when >= 0; otherwise follows FBCON_LOGO
    enums.  */
@@ -607,6 +605,7 @@ static void fbcon_prepare_logo(struct vc_data *vc, struct fb_info *info,
 	struct fbcon_ops *ops = info->fbcon_par;
 	int cnt, erase = vc->vc_video_erase_char, step;
 	unsigned short *save = NULL, *r, *q;
+	int logo_height;
 
 	if (info->flags & FBINFO_MODULE) {
 		logo_shown = FBCON_LOGO_DONTSHOW;
diff --git a/drivers/video/console/fbcon.h b/drivers/video/console/fbcon.h
index 0135e039545..de1b1365279 100644
--- a/drivers/video/console/fbcon.h
+++ b/drivers/video/console/fbcon.h
@@ -92,7 +92,7 @@ struct fbcon_ops {
 #define attr_fgcol(fgshift,s)    \
 	(((s) >> (fgshift)) & 0x0f)
 #define attr_bgcol(bgshift,s)    \
-	(((s) >> (bgshift)) & 0x0f)
+	(((s) >> (bgshift)) & 0x07)
 
 /* Monochrome */
 #define attr_bold(s) \
@@ -146,10 +146,8 @@ static inline int attr_col_ec(int shift, struct vc_data *vc,
 	return is_fg ? fg : bg;
 }
 
-#define attr_bgcol_ec(bgshift,vc,info)		\
-	attr_col_ec(bgshift,vc,info,0);
-#define attr_fgcol_ec(fgshift,vc,info)		\
-	attr_col_ec(fgshift,vc,info,1);
+#define attr_bgcol_ec(bgshift, vc, info) attr_col_ec(bgshift, vc, info, 0)
+#define attr_fgcol_ec(fgshift, vc, info) attr_col_ec(fgshift, vc, info, 1)
 
 /* Font */
 #define REFCOUNT(fd)	(((int *)(fd))[-1])
diff --git a/drivers/video/console/mdacon.c b/drivers/video/console/mdacon.c
index 38a296bbdfc..9901064199b 100644
--- a/drivers/video/console/mdacon.c
+++ b/drivers/video/console/mdacon.c
@@ -71,13 +71,15 @@ static char *mda_type_name;
 
 /* console information */
 
-static int	mda_first_vc = 1;
+static int	mda_first_vc = 13;
 static int	mda_last_vc  = 16;
 
 static struct vc_data	*mda_display_fg = NULL;
 
 module_param(mda_first_vc, int, 0);
+MODULE_PARM_DESC(mda_first_vc, "First virtual console. Default: 13");
 module_param(mda_last_vc, int, 0);
+MODULE_PARM_DESC(mda_last_vc, "Last virtual console. Default: 16");
 
 /* MDA register values
  */
diff --git a/drivers/video/console/sticon.c b/drivers/video/console/sticon.c
index a11cc2fdd4c..4055dbdd1b4 100644
--- a/drivers/video/console/sticon.c
+++ b/drivers/video/console/sticon.c
@@ -370,7 +370,7 @@ static const struct consw sti_con = {
 
 
 
-int __init sticonsole_init(void)
+static int __init sticonsole_init(void)
 {
     /* already initialized ? */
     if (sticon_sti)
diff --git a/drivers/video/console/sticore.c b/drivers/video/console/sticore.c
index e9ab657f0bb..d7822af0e00 100644
--- a/drivers/video/console/sticore.c
+++ b/drivers/video/console/sticore.c
@@ -29,7 +29,7 @@
 
 #define STI_DRIVERVERSION "Version 0.9a"
 
-struct sti_struct *default_sti __read_mostly;
+static struct sti_struct *default_sti __read_mostly;
 
 /* number of STI ROMS found and their ptrs to each struct */
 static int num_sti_roms __read_mostly;
@@ -68,8 +68,7 @@ static const struct sti_init_flags default_init_flags = {
 	.init_cmap_tx = 1,
 };
 
-int
-sti_init_graph(struct sti_struct *sti) 
+static int sti_init_graph(struct sti_struct *sti)
 {
 	struct sti_init_inptr_ext inptr_ext = { 0, };
 	struct sti_init_inptr inptr = {
@@ -100,8 +99,7 @@ static const struct sti_conf_flags default_conf_flags = {
 	.wait	= STI_WAIT,
 };
 
-void
-sti_inq_conf(struct sti_struct *sti)
+static void sti_inq_conf(struct sti_struct *sti)
 {
 	struct sti_conf_inptr inptr = { 0, };
 	unsigned long flags;
@@ -237,8 +235,8 @@ static void sti_flush(unsigned long start, unsigned long end)
 	flush_icache_range(start, end);
 }
 
-void __devinit
-sti_rom_copy(unsigned long base, unsigned long count, void *dest)
+static void __devinit sti_rom_copy(unsigned long base, unsigned long count,
+				   void *dest)
 {
 	unsigned long dest_start = (unsigned long) dest;
 
@@ -478,8 +476,8 @@ sti_init_glob_cfg(struct sti_struct *sti,
 }
 
 #ifdef CONFIG_FB
-struct sti_cooked_font * __devinit
-sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
+static struct sti_cooked_font __devinit
+*sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
 {
 	const struct font_desc *fbfont;
 	unsigned int size, bpc;
@@ -534,16 +532,16 @@ sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
 	return cooked_font;
 }
 #else
-struct sti_cooked_font * __devinit
-sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
+static struct sti_cooked_font __devinit
+*sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
 {
 	return NULL;
 }
 #endif
 
-struct sti_cooked_font * __devinit
-sti_select_font(struct sti_cooked_rom *rom,
-	    int (*search_font_fnc) (struct sti_cooked_rom *,int,int) )
+static struct sti_cooked_font __devinit
+*sti_select_font(struct sti_cooked_rom *rom,
+		 int (*search_font_fnc)(struct sti_cooked_rom *, int, int))
 {
 	struct sti_cooked_font *font;
 	int i;
@@ -707,8 +705,7 @@ sti_get_bmode_rom (unsigned long address)
 	return raw;
 }
 
-struct sti_rom * __devinit
-sti_get_wmode_rom (unsigned long address)
+static struct sti_rom __devinit *sti_get_wmode_rom(unsigned long address)
 {
 	struct sti_rom *raw;
 	unsigned long size;
@@ -723,8 +720,8 @@ sti_get_wmode_rom (unsigned long address)
 	return raw;
 }
 
-int __devinit
-sti_read_rom(int wordmode, struct sti_struct *sti, unsigned long address)
+static int __devinit sti_read_rom(int wordmode, struct sti_struct *sti,
+				  unsigned long address)
 {
 	struct sti_cooked_rom *cooked;
 	struct sti_rom *raw = NULL;
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 1cd5071e536..6b487801eea 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -848,9 +848,8 @@ int
 fb_pan_display(struct fb_info *info, struct fb_var_screeninfo *var)
 {
 	struct fb_fix_screeninfo *fix = &info->fix;
-        int xoffset = var->xoffset;
-        int yoffset = var->yoffset;
-        int err = 0, yres = info->var.yres;
+	unsigned int yres = info->var.yres;
+	int err = 0;
 
 	if (var->yoffset > 0) {
 		if (var->vmode & FB_VMODE_YWRAP) {
@@ -866,8 +865,8 @@ fb_pan_display(struct fb_info *info, struct fb_var_screeninfo *var)
 				 (var->xoffset % fix->xpanstep)))
 		err = -EINVAL;
 
-        if (err || !info->fbops->fb_pan_display || xoffset < 0 ||
-	    yoffset < 0 || var->yoffset + yres > info->var.yres_virtual ||
+	if (err || !info->fbops->fb_pan_display ||
+	    var->yoffset + yres > info->var.yres_virtual ||
 	    var->xoffset + info->var.xres > info->var.xres_virtual)
 		return -EINVAL;
 
diff --git a/drivers/video/fbmon.c b/drivers/video/fbmon.c
index 052e1805849..6a0aa180c26 100644
--- a/drivers/video/fbmon.c
+++ b/drivers/video/fbmon.c
@@ -879,7 +879,7 @@ int fb_parse_edid(unsigned char *edid, struct fb_var_screeninfo *var)
 		if (edid_is_timing_block(block)) {
 			var->xres = var->xres_virtual = H_ACTIVE;
 			var->yres = var->yres_virtual = V_ACTIVE;
-			var->height = var->width = -1;
+			var->height = var->width = 0;
 			var->right_margin = H_SYNC_OFFSET;
 			var->left_margin = (H_ACTIVE + H_BLANKING) -
 				(H_ACTIVE + H_SYNC_OFFSET + H_SYNC_WIDTH);
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 09d7e22c6fe..9cd36c223d3 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -279,58 +279,42 @@ static struct diu_hw dr = {
 
 static struct diu_pool pool;
 
-/*	To allocate memory for framebuffer. First try __get_free_pages(). If it
- *	fails, try rh_alloc. The reason is __get_free_pages() cannot allocate
- *	very large memory (more than 4MB). We don't want to allocate all memory
- *	in rheap since small memory allocation/deallocation will fragment the
- *	rheap and make the furture large allocation fail.
+/**
+ * fsl_diu_alloc - allocate memory for the DIU
+ * @size: number of bytes to allocate
+ * @param: returned physical address of memory
+ *
+ * This function allocates a physically-contiguous block of memory.
  */
-
-static void *fsl_diu_alloc(unsigned long size, phys_addr_t *phys)
+static void *fsl_diu_alloc(size_t size, phys_addr_t *phys)
 {
 	void *virt;
 
-	pr_debug("size=%lu\n", size);
+	pr_debug("size=%zu\n", size);
 
-	virt = (void *)__get_free_pages(GFP_DMA | __GFP_ZERO, get_order(size));
+	virt = alloc_pages_exact(size, GFP_DMA | __GFP_ZERO);
 	if (virt) {
 		*phys = virt_to_phys(virt);
-		pr_debug("virt %p, phys=%llx\n", virt, (uint64_t) *phys);
-		return virt;
-	}
-	if (!diu_ops.diu_mem) {
-		printk(KERN_INFO "%s: no diu_mem."
-			" To reserve more memory, put 'diufb=15M' "
-			"in the command line\n", __func__);
-		return NULL;
-	}
-
-	virt = (void *)rh_alloc(&diu_ops.diu_rh_info, size, "DIU");
-	if (virt) {
-		*phys = virt_to_bus(virt);
-		memset(virt, 0, size);
+		pr_debug("virt=%p phys=%llx\n", virt,
+			(unsigned long long)*phys);
 	}
 
-	pr_debug("rh virt=%p phys=%llx\n", virt, (unsigned long long)*phys);
-
 	return virt;
 }
 
-static void fsl_diu_free(void *p, unsigned long size)
+/**
+ * fsl_diu_free - release DIU memory
+ * @virt: pointer returned by fsl_diu_alloc()
+ * @size: number of bytes allocated by fsl_diu_alloc()
+ *
+ * This function releases memory allocated by fsl_diu_alloc().
+ */
+static void fsl_diu_free(void *virt, size_t size)
 {
-	pr_debug("p=%p size=%lu\n", p, size);
+	pr_debug("virt=%p size=%zu\n", virt, size);
 
-	if (!p)
-		return;
-
-	if ((p >= diu_ops.diu_mem) &&
-	    (p < (diu_ops.diu_mem + diu_ops.diu_size))) {
-		pr_debug("rh\n");
-		rh_free(&diu_ops.diu_rh_info, (unsigned long) p);
-	} else {
-		pr_debug("dma\n");
-		free_pages((unsigned long)p, get_order(size));
-	}
+	if (virt && size)
+		free_pages_exact(virt, size);
 }
 
 static int fsl_diu_enable_panel(struct fb_info *info)
diff --git a/drivers/video/geode/lxfb.h b/drivers/video/geode/lxfb.h
index 3b9416f4ee2..6a51448fd3f 100644
--- a/drivers/video/geode/lxfb.h
+++ b/drivers/video/geode/lxfb.h
@@ -51,8 +51,6 @@ static inline unsigned int lx_get_pitch(unsigned int xres, int bpp)
 }
 
 void lx_set_mode(struct fb_info *);
-void lx_get_gamma(struct fb_info *, unsigned int *, int);
-void lx_set_gamma(struct fb_info *, unsigned int *, int);
 unsigned int lx_framebuffer_size(void);
 int lx_blank_display(struct fb_info *, int);
 void lx_set_palette_reg(struct fb_info *, unsigned int, unsigned int,
diff --git a/drivers/video/geode/lxfb_ops.c b/drivers/video/geode/lxfb_ops.c
index aaef9165ec9..b1cd49c9935 100644
--- a/drivers/video/geode/lxfb_ops.c
+++ b/drivers/video/geode/lxfb_ops.c
@@ -517,25 +517,25 @@ void lx_set_palette_reg(struct fb_info *info, unsigned regno,
 int lx_blank_display(struct fb_info *info, int blank_mode)
 {
 	struct lxfb_par *par = info->par;
-	u32 dcfg, fp_pm;
-	int blank, hsync, vsync, crt;
+	u32 dcfg, misc, fp_pm;
+	int blank, hsync, vsync;
 
 	/* CRT power saving modes. */
 	switch (blank_mode) {
 	case FB_BLANK_UNBLANK:
-		blank = 0; hsync = 1; vsync = 1; crt = 1;
+		blank = 0; hsync = 1; vsync = 1;
 		break;
 	case FB_BLANK_NORMAL:
-		blank = 1; hsync = 1; vsync = 1; crt = 1;
+		blank = 1; hsync = 1; vsync = 1;
 		break;
 	case FB_BLANK_VSYNC_SUSPEND:
-		blank = 1; hsync = 1; vsync = 0; crt = 1;
+		blank = 1; hsync = 1; vsync = 0;
 		break;
 	case FB_BLANK_HSYNC_SUSPEND:
-		blank = 1; hsync = 0; vsync = 1; crt = 1;
+		blank = 1; hsync = 0; vsync = 1;
 		break;
 	case FB_BLANK_POWERDOWN:
-		blank = 1; hsync = 0; vsync = 0; crt = 0;
+		blank = 1; hsync = 0; vsync = 0;
 		break;
 	default:
 		return -EINVAL;
@@ -545,15 +545,23 @@ int lx_blank_display(struct fb_info *info, int blank_mode)
 	dcfg &= ~(VP_DCFG_DAC_BL_EN | VP_DCFG_HSYNC_EN | VP_DCFG_VSYNC_EN |
 			VP_DCFG_CRT_EN);
 	if (!blank)
-		dcfg |= VP_DCFG_DAC_BL_EN;
+		dcfg |= VP_DCFG_DAC_BL_EN | VP_DCFG_CRT_EN;
 	if (hsync)
 		dcfg |= VP_DCFG_HSYNC_EN;
 	if (vsync)
 		dcfg |= VP_DCFG_VSYNC_EN;
-	if (crt)
-		dcfg |= VP_DCFG_CRT_EN;
+
 	write_vp(par, VP_DCFG, dcfg);
 
+	misc = read_vp(par, VP_MISC);
+
+	if (vsync && hsync)
+		misc &= ~VP_MISC_DACPWRDN;
+	else
+		misc |= VP_MISC_DACPWRDN;
+
+	write_vp(par, VP_MISC, misc);
+
 	/* Power on/off flat panel */
 
 	if (par->output & OUTPUT_PANEL) {
diff --git a/drivers/video/hgafb.c b/drivers/video/hgafb.c
index c18880d9db1..0129c044f6d 100644
--- a/drivers/video/hgafb.c
+++ b/drivers/video/hgafb.c
@@ -551,7 +551,7 @@ static struct fb_ops hgafb_ops = {
 	 *  Initialization
 	 */
 
-static int __init hgafb_probe(struct device *device)
+static int __init hgafb_probe(struct platform_device *pdev)
 {
 	struct fb_info *info;
 
@@ -565,7 +565,7 @@ static int __init hgafb_probe(struct device *device)
 	printk(KERN_INFO "hgafb: %s with %ldK of memory detected.\n",
 		hga_type_name, hga_vram_len/1024);
 
-	info = framebuffer_alloc(0, NULL);
+	info = framebuffer_alloc(0, &pdev->dev);
 	if (!info) {
 		iounmap(hga_vram);
 		return -ENOMEM;
@@ -593,13 +593,13 @@ static int __init hgafb_probe(struct device *device)
 
         printk(KERN_INFO "fb%d: %s frame buffer device\n",
                info->node, info->fix.id);
-	dev_set_drvdata(device, info);
+	platform_set_drvdata(pdev, info);
 	return 0;
 }
 
-static int hgafb_remove(struct device *device)
+static int hgafb_remove(struct platform_device *pdev)
 {
-	struct fb_info *info = dev_get_drvdata(device);
+	struct fb_info *info = platform_get_drvdata(pdev);
 
 	hga_txt_mode();
 	hga_clear_screen();
@@ -620,16 +620,15 @@ static int hgafb_remove(struct device *device)
 	return 0;
 }
 
-static struct device_driver hgafb_driver = {
-	.name = "hgafb",
-	.bus  = &platform_bus_type,
+static struct platform_driver hgafb_driver = {
 	.probe = hgafb_probe,
 	.remove = hgafb_remove,
+	.driver = {
+		.name = "hgafb",
+	},
 };
 
-static struct platform_device hgafb_device = {
-	.name = "hgafb",
-};
+static struct platform_device *hgafb_device;
 
 static int __init hgafb_init(void)
 {
@@ -638,12 +637,15 @@ static int __init hgafb_init(void)
 	if (fb_get_options("hgafb", NULL))
 		return -ENODEV;
 
-	ret = driver_register(&hgafb_driver);
+	ret = platform_driver_register(&hgafb_driver);
 
 	if (!ret) {
-		ret = platform_device_register(&hgafb_device);
-		if (ret)
-			driver_unregister(&hgafb_driver);
+		hgafb_device = platform_device_register_simple("hgafb", 0, NULL, 0);
+
+		if (IS_ERR(hgafb_device)) {
+			platform_driver_unregister(&hgafb_driver);
+			ret = PTR_ERR(hgafb_device);
+		}
 	}
 
 	return ret;
@@ -651,8 +653,8 @@ static int __init hgafb_init(void)
 
 static void __exit hgafb_exit(void)
 {
-	platform_device_unregister(&hgafb_device);
-	driver_unregister(&hgafb_driver);
+	platform_device_unregister(hgafb_device);
+	platform_driver_unregister(&hgafb_driver);
 }
 
 /* -------------------------------------------------------------------------
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 94e4d3ac1a0..0c5a475c1ca 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -24,6 +24,7 @@
 #include <linux/string.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/fb.h>
 #include <linux/delay.h>
 #include <linux/init.h>
diff --git a/drivers/video/macfb.c b/drivers/video/macfb.c
index aa8c714d624..b790ddff76f 100644
--- a/drivers/video/macfb.c
+++ b/drivers/video/macfb.c
@@ -596,7 +596,7 @@ static struct fb_ops macfb_ops = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
-void __init macfb_setup(char *options)
+static void __init macfb_setup(char *options)
 {
 	char *this_opt;
 	
diff --git a/drivers/video/neofb.c b/drivers/video/neofb.c
index 5246b0402d7..25172b2a2a9 100644
--- a/drivers/video/neofb.c
+++ b/drivers/video/neofb.c
@@ -201,7 +201,6 @@ static int neoFindMode(int xres, int yres, int depth)
  *
  * Determine the closest clock frequency to the one requested.
  */
-#define REF_FREQ 0xe517		/* 14.31818 in 20.12 fixed point */
 #define MAX_N 127
 #define MAX_D 31
 #define MAX_F 1
@@ -211,27 +210,24 @@ static void neoCalcVCLK(const struct fb_info *info,
 {
 	int n, d, f;
 	int n_best = 0, d_best = 0, f_best = 0;
-	long f_best_diff = (0x7ffff << 12);	/* 20.12 */
-	long f_target = (freq << 12) / 1000;	/* 20.12 */
+	long f_best_diff = 0x7ffff;
 
 	for (f = 0; f <= MAX_F; f++)
-		for (n = 0; n <= MAX_N; n++)
-			for (d = 0; d <= MAX_D; d++) {
-				long f_out;	/* 20.12 */
-				long f_diff;	/* 20.12 */
-
-				f_out =
-				    ((((n + 1) << 12) / ((d +
-							  1) *
-							 (1 << f))) >> 12)
-				    * REF_FREQ;
-				f_diff = abs(f_out - f_target);
-				if (f_diff < f_best_diff) {
+		for (d = 0; d <= MAX_D; d++)
+			for (n = 0; n <= MAX_N; n++) {
+				long f_out;
+				long f_diff;
+
+				f_out = ((14318 * (n + 1)) / (d + 1)) >> f;
+				f_diff = abs(f_out - freq);
+				if (f_diff <= f_best_diff) {
 					f_best_diff = f_diff;
 					n_best = n;
 					d_best = d;
 					f_best = f;
 				}
+				if (f_out > freq)
+					break;
 			}
 
 	if (info->fix.accel == FB_ACCEL_NEOMAGIC_NM2200 ||
@@ -248,11 +244,11 @@ static void neoCalcVCLK(const struct fb_info *info,
 	par->VCLK3Denominator = d_best;
 
 #ifdef NEOFB_DEBUG
-	printk("neoVCLK: f:%d NumLow=%d NumHi=%d Den=%d Df=%d\n",
-	       f_target >> 12,
+	printk(KERN_DEBUG "neoVCLK: f:%ld NumLow=%d NumHi=%d Den=%d Df=%ld\n",
+	       freq,
 	       par->VCLK3NumeratorLow,
 	       par->VCLK3NumeratorHigh,
-	       par->VCLK3Denominator, f_best_diff >> 12);
+	       par->VCLK3Denominator, f_best_diff);
 #endif
 }
 
@@ -263,15 +259,20 @@ static void neoCalcVCLK(const struct fb_info *info,
  */
 
 static int vgaHWInit(const struct fb_var_screeninfo *var,
-		     const struct fb_info *info,
-		     struct neofb_par *par, struct xtimings *timings)
+		     struct neofb_par *par)
 {
+	int hsync_end = var->xres + var->right_margin + var->hsync_len;
+	int htotal = (hsync_end + var->left_margin) >> 3;
+	int vsync_start = var->yres + var->lower_margin;
+	int vsync_end = vsync_start + var->vsync_len;
+	int vtotal = vsync_end + var->upper_margin;
+
 	par->MiscOutReg = 0x23;
 
-	if (!(timings->sync & FB_SYNC_HOR_HIGH_ACT))
+	if (!(var->sync & FB_SYNC_HOR_HIGH_ACT))
 		par->MiscOutReg |= 0x40;
 
-	if (!(timings->sync & FB_SYNC_VERT_HIGH_ACT))
+	if (!(var->sync & FB_SYNC_VERT_HIGH_ACT))
 		par->MiscOutReg |= 0x80;
 
 	/*
@@ -286,25 +287,25 @@ static int vgaHWInit(const struct fb_var_screeninfo *var,
 	/*
 	 * CRTC Controller
 	 */
-	par->CRTC[0] = (timings->HTotal >> 3) - 5;
-	par->CRTC[1] = (timings->HDisplay >> 3) - 1;
-	par->CRTC[2] = (timings->HDisplay >> 3) - 1;
-	par->CRTC[3] = (((timings->HTotal >> 3) - 1) & 0x1F) | 0x80;
-	par->CRTC[4] = (timings->HSyncStart >> 3);
-	par->CRTC[5] = ((((timings->HTotal >> 3) - 1) & 0x20) << 2)
-	    | (((timings->HSyncEnd >> 3)) & 0x1F);
-	par->CRTC[6] = (timings->VTotal - 2) & 0xFF;
-	par->CRTC[7] = (((timings->VTotal - 2) & 0x100) >> 8)
-	    | (((timings->VDisplay - 1) & 0x100) >> 7)
-	    | ((timings->VSyncStart & 0x100) >> 6)
-	    | (((timings->VDisplay - 1) & 0x100) >> 5)
-	    | 0x10 | (((timings->VTotal - 2) & 0x200) >> 4)
-	    | (((timings->VDisplay - 1) & 0x200) >> 3)
-	    | ((timings->VSyncStart & 0x200) >> 2);
+	par->CRTC[0] = htotal - 5;
+	par->CRTC[1] = (var->xres >> 3) - 1;
+	par->CRTC[2] = (var->xres >> 3) - 1;
+	par->CRTC[3] = ((htotal - 1) & 0x1F) | 0x80;
+	par->CRTC[4] = ((var->xres + var->right_margin) >> 3);
+	par->CRTC[5] = (((htotal - 1) & 0x20) << 2)
+	    | (((hsync_end >> 3)) & 0x1F);
+	par->CRTC[6] = (vtotal - 2) & 0xFF;
+	par->CRTC[7] = (((vtotal - 2) & 0x100) >> 8)
+	    | (((var->yres - 1) & 0x100) >> 7)
+	    | ((vsync_start & 0x100) >> 6)
+	    | (((var->yres - 1) & 0x100) >> 5)
+	    | 0x10 | (((vtotal - 2) & 0x200) >> 4)
+	    | (((var->yres - 1) & 0x200) >> 3)
+	    | ((vsync_start & 0x200) >> 2);
 	par->CRTC[8] = 0x00;
-	par->CRTC[9] = (((timings->VDisplay - 1) & 0x200) >> 4) | 0x40;
+	par->CRTC[9] = (((var->yres - 1) & 0x200) >> 4) | 0x40;
 
-	if (timings->dblscan)
+	if (var->vmode & FB_VMODE_DOUBLE)
 		par->CRTC[9] |= 0x80;
 
 	par->CRTC[10] = 0x00;
@@ -313,13 +314,13 @@ static int vgaHWInit(const struct fb_var_screeninfo *var,
 	par->CRTC[13] = 0x00;
 	par->CRTC[14] = 0x00;
 	par->CRTC[15] = 0x00;
-	par->CRTC[16] = timings->VSyncStart & 0xFF;
-	par->CRTC[17] = (timings->VSyncEnd & 0x0F) | 0x20;
-	par->CRTC[18] = (timings->VDisplay - 1) & 0xFF;
+	par->CRTC[16] = vsync_start & 0xFF;
+	par->CRTC[17] = (vsync_end & 0x0F) | 0x20;
+	par->CRTC[18] = (var->yres - 1) & 0xFF;
 	par->CRTC[19] = var->xres_virtual >> 4;
 	par->CRTC[20] = 0x00;
-	par->CRTC[21] = (timings->VDisplay - 1) & 0xFF;
-	par->CRTC[22] = (timings->VTotal - 1) & 0xFF;
+	par->CRTC[21] = (var->yres - 1) & 0xFF;
+	par->CRTC[22] = (vtotal - 1) & 0xFF;
 	par->CRTC[23] = 0xC3;
 	par->CRTC[24] = 0xFF;
 
@@ -483,7 +484,8 @@ static inline int neo2200_sync(struct fb_info *info)
 {
 	struct neofb_par *par = info->par;
 
-	while (readl(&par->neo2200->bltStat) & 1);
+	while (readl(&par->neo2200->bltStat) & 1)
+		cpu_relax();
 	return 0;
 }
 
@@ -591,34 +593,14 @@ static int
 neofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	struct neofb_par *par = info->par;
-	unsigned int pixclock = var->pixclock;
-	struct xtimings timings;
 	int memlen, vramlen;
 	int mode_ok = 0;
 
 	DBG("neofb_check_var");
 
-	if (!pixclock)
-		pixclock = 10000;	/* 10ns = 100MHz */
-	timings.pixclock = 1000000000 / pixclock;
-	if (timings.pixclock < 1)
-		timings.pixclock = 1;
-
-	if (timings.pixclock > par->maxClock)
+	if (PICOS2KHZ(var->pixclock) > par->maxClock)
 		return -EINVAL;
 
-	timings.dblscan = var->vmode & FB_VMODE_DOUBLE;
-	timings.interlaced = var->vmode & FB_VMODE_INTERLACED;
-	timings.HDisplay = var->xres;
-	timings.HSyncStart = timings.HDisplay + var->right_margin;
-	timings.HSyncEnd = timings.HSyncStart + var->hsync_len;
-	timings.HTotal = timings.HSyncEnd + var->left_margin;
-	timings.VDisplay = var->yres;
-	timings.VSyncStart = timings.VDisplay + var->lower_margin;
-	timings.VSyncEnd = timings.VSyncStart + var->vsync_len;
-	timings.VTotal = timings.VSyncEnd + var->upper_margin;
-	timings.sync = var->sync;
-
 	/* Is the mode larger than the LCD panel? */
 	if (par->internal_display &&
             ((var->xres > par->NeoPanelWidth) ||
@@ -759,11 +741,11 @@ neofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 static int neofb_set_par(struct fb_info *info)
 {
 	struct neofb_par *par = info->par;
-	struct xtimings timings;
 	unsigned char temp;
 	int i, clock_hi = 0;
 	int lcd_stretch;
 	int hoffset, voffset;
+	int vsync_start, vtotal;
 
 	DBG("neofb_set_par");
 
@@ -771,28 +753,15 @@ static int neofb_set_par(struct fb_info *info)
 
 	vgaHWProtect(1);	/* Blank the screen */
 
-	timings.dblscan = info->var.vmode & FB_VMODE_DOUBLE;
-	timings.interlaced = info->var.vmode & FB_VMODE_INTERLACED;
-	timings.HDisplay = info->var.xres;
-	timings.HSyncStart = timings.HDisplay + info->var.right_margin;
-	timings.HSyncEnd = timings.HSyncStart + info->var.hsync_len;
-	timings.HTotal = timings.HSyncEnd + info->var.left_margin;
-	timings.VDisplay = info->var.yres;
-	timings.VSyncStart = timings.VDisplay + info->var.lower_margin;
-	timings.VSyncEnd = timings.VSyncStart + info->var.vsync_len;
-	timings.VTotal = timings.VSyncEnd + info->var.upper_margin;
-	timings.sync = info->var.sync;
-	timings.pixclock = PICOS2KHZ(info->var.pixclock);
-
-	if (timings.pixclock < 1)
-		timings.pixclock = 1;
+	vsync_start = info->var.yres + info->var.lower_margin;
+	vtotal = vsync_start + info->var.vsync_len + info->var.upper_margin;
 
 	/*
 	 * This will allocate the datastructure and initialize all of the
 	 * generic VGA registers.
 	 */
 
-	if (vgaHWInit(&info->var, info, par, &timings))
+	if (vgaHWInit(&info->var, par))
 		return -EINVAL;
 
 	/*
@@ -831,10 +800,10 @@ static int neofb_set_par(struct fb_info *info)
 	par->ExtCRTDispAddr = 0x10;
 
 	/* Vertical Extension */
-	par->VerticalExt = (((timings.VTotal - 2) & 0x400) >> 10)
-	    | (((timings.VDisplay - 1) & 0x400) >> 9)
-	    | (((timings.VSyncStart) & 0x400) >> 8)
-	    | (((timings.VSyncStart) & 0x400) >> 7);
+	par->VerticalExt = (((vtotal - 2) & 0x400) >> 10)
+	    | (((info->var.yres - 1) & 0x400) >> 9)
+	    | (((vsync_start) & 0x400) >> 8)
+	    | (((vsync_start) & 0x400) >> 7);
 
 	/* Fast write bursts on unless disabled. */
 	if (par->pci_burst)
@@ -995,7 +964,7 @@ static int neofb_set_par(struct fb_info *info)
 	 * Calculate the VCLK that most closely matches the requested dot
 	 * clock.
 	 */
-	neoCalcVCLK(info, par, timings.pixclock);
+	neoCalcVCLK(info, par, PICOS2KHZ(info->var.pixclock));
 
 	/* Since we program the clocks ourselves, always use VCLK3. */
 	par->MiscOutReg |= 0x0C;
@@ -1927,9 +1896,6 @@ static int __devinit neo_init_hw(struct fb_info *info)
 	int maxClock = 65000;
 	int CursorMem = 1024;
 	int CursorOff = 0x100;
-	int linearSize = 1024;
-	int maxWidth = 1024;
-	int maxHeight = 1024;
 
 	DBG("neo_init_hw");
 
@@ -1948,81 +1914,52 @@ static int __devinit neo_init_hw(struct fb_info *info)
 	case FB_ACCEL_NEOMAGIC_NM2070:
 		videoRam = 896;
 		maxClock = 65000;
-		CursorMem = 2048;
-		CursorOff = 0x100;
-		linearSize = 1024;
-		maxWidth = 1024;
-		maxHeight = 1024;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2090:
 	case FB_ACCEL_NEOMAGIC_NM2093:
-		videoRam = 1152;
-		maxClock = 80000;
-		CursorMem = 2048;
-		CursorOff = 0x100;
-		linearSize = 2048;
-		maxWidth = 1024;
-		maxHeight = 1024;
-		break;
 	case FB_ACCEL_NEOMAGIC_NM2097:
 		videoRam = 1152;
 		maxClock = 80000;
-		CursorMem = 1024;
-		CursorOff = 0x100;
-		linearSize = 2048;
-		maxWidth = 1024;
-		maxHeight = 1024;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2160:
 		videoRam = 2048;
 		maxClock = 90000;
-		CursorMem = 1024;
-		CursorOff = 0x100;
-		linearSize = 2048;
-		maxWidth = 1024;
-		maxHeight = 1024;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2200:
 		videoRam = 2560;
 		maxClock = 110000;
-		CursorMem = 1024;
-		CursorOff = 0x1000;
-		linearSize = 4096;
-		maxWidth = 1280;
-		maxHeight = 1024;	/* ???? */
-
-		par->neo2200 = (Neo2200 __iomem *) par->mmio_vbase;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2230:
 		videoRam = 3008;
 		maxClock = 110000;
-		CursorMem = 1024;
-		CursorOff = 0x1000;
-		linearSize = 4096;
-		maxWidth = 1280;
-		maxHeight = 1024;	/* ???? */
-
-		par->neo2200 = (Neo2200 __iomem *) par->mmio_vbase;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2360:
 		videoRam = 4096;
 		maxClock = 110000;
-		CursorMem = 1024;
-		CursorOff = 0x1000;
-		linearSize = 4096;
-		maxWidth = 1280;
-		maxHeight = 1024;	/* ???? */
-
-		par->neo2200 = (Neo2200 __iomem *) par->mmio_vbase;
 		break;
 	case FB_ACCEL_NEOMAGIC_NM2380:
 		videoRam = 6144;
 		maxClock = 110000;
+		break;
+	}
+	switch (info->fix.accel) {
+	case FB_ACCEL_NEOMAGIC_NM2070:
+	case FB_ACCEL_NEOMAGIC_NM2090:
+	case FB_ACCEL_NEOMAGIC_NM2093:
+		CursorMem = 2048;
+		CursorOff = 0x100;
+		break;
+	case FB_ACCEL_NEOMAGIC_NM2097:
+	case FB_ACCEL_NEOMAGIC_NM2160:
+		CursorMem = 1024;
+		CursorOff = 0x100;
+		break;
+	case FB_ACCEL_NEOMAGIC_NM2200:
+	case FB_ACCEL_NEOMAGIC_NM2230:
+	case FB_ACCEL_NEOMAGIC_NM2360:
+	case FB_ACCEL_NEOMAGIC_NM2380:
 		CursorMem = 1024;
 		CursorOff = 0x1000;
-		linearSize = 8192;
-		maxWidth = 1280;
-		maxHeight = 1024;	/* ???? */
 
 		par->neo2200 = (Neo2200 __iomem *) par->mmio_vbase;
 		break;
@@ -2036,7 +1973,7 @@ static int __devinit neo_init_hw(struct fb_info *info)
 */
 	par->maxClock = maxClock;
 	par->cursorOff = CursorOff;
-	return ((videoRam * 1024));
+	return videoRam * 1024;
 }
 
 
diff --git a/drivers/video/omap/dispc.c b/drivers/video/omap/dispc.c
index ab32ceb0617..ab77c51fe9d 100644
--- a/drivers/video/omap/dispc.c
+++ b/drivers/video/omap/dispc.c
@@ -20,6 +20,7 @@
  */
 #include <linux/kernel.h>
 #include <linux/dma-mapping.h>
+#include <linux/mm.h>
 #include <linux/vmalloc.h>
 #include <linux/clk.h>
 #include <linux/io.h>
diff --git a/drivers/video/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index 14d0f7a1114..f85af5c4fa6 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -25,6 +25,7 @@
  * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  */
 #include <linux/platform_device.h>
+#include <linux/mm.h>
 #include <linux/uaccess.h>
 
 #include <asm/mach-types.h>
diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index bb251436950..2b707a8ce5d 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -30,6 +30,7 @@
 #include <linux/string.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/fb.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -40,6 +41,7 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/completion.h>
+#include <linux/mutex.h>
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 
@@ -1116,7 +1118,7 @@ static void set_ctrlr_state(struct pxafb_info *fbi, u_int state)
 {
 	u_int old_state;
 
-	down(&fbi->ctrlr_sem);
+	mutex_lock(&fbi->ctrlr_lock);
 
 	old_state = fbi->state;
 
@@ -1204,7 +1206,7 @@ static void set_ctrlr_state(struct pxafb_info *fbi, u_int state)
 		}
 		break;
 	}
-	up(&fbi->ctrlr_sem);
+	mutex_unlock(&fbi->ctrlr_lock);
 }
 
 /*
@@ -1457,7 +1459,7 @@ static struct pxafb_info * __devinit pxafb_init_fbinfo(struct device *dev)
 
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, pxafb_task);
-	init_MUTEX(&fbi->ctrlr_sem);
+	mutex_init(&fbi->ctrlr_lock);
 	init_completion(&fbi->disable_done);
 #ifdef CONFIG_FB_PXA_SMARTPANEL
 	init_completion(&fbi->command_done);
diff --git a/drivers/video/pxafb.h b/drivers/video/pxafb.h
index 8238dc82642..31541b86f13 100644
--- a/drivers/video/pxafb.h
+++ b/drivers/video/pxafb.h
@@ -106,7 +106,7 @@ struct pxafb_info {
 
 	volatile u_char		state;
 	volatile u_char		task_state;
-	struct semaphore	ctrlr_sem;
+	struct mutex		ctrlr_lock;
 	wait_queue_head_t	ctrlr_wait;
 	struct work_struct	task;
 
diff --git a/drivers/video/sa1100fb.c b/drivers/video/sa1100fb.c
index ab2b2110478..78bcdbc3f48 100644
--- a/drivers/video/sa1100fb.c
+++ b/drivers/video/sa1100fb.c
@@ -167,6 +167,7 @@
 #include <linux/string.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/fb.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -174,6 +175,7 @@
 #include <linux/cpufreq.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
+#include <linux/mutex.h>
 
 #include <asm/hardware.h>
 #include <asm/io.h>
@@ -1107,7 +1109,7 @@ static void set_ctrlr_state(struct sa1100fb_info *fbi, u_int state)
 {
 	u_int old_state;
 
-	down(&fbi->ctrlr_sem);
+	mutex_lock(&fbi->ctrlr_lock);
 
 	old_state = fbi->state;
 
@@ -1192,7 +1194,7 @@ static void set_ctrlr_state(struct sa1100fb_info *fbi, u_int state)
 		}
 		break;
 	}
-	up(&fbi->ctrlr_sem);
+	mutex_unlock(&fbi->ctrlr_lock);
 }
 
 /*
@@ -1444,7 +1446,7 @@ static struct sa1100fb_info * __init sa1100fb_init_fbinfo(struct device *dev)
 
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, sa1100fb_task);
-	init_MUTEX(&fbi->ctrlr_sem);
+	mutex_init(&fbi->ctrlr_lock);
 
 	return fbi;
 }
diff --git a/drivers/video/sa1100fb.h b/drivers/video/sa1100fb.h
index f465b27ed86..86831db9a04 100644
--- a/drivers/video/sa1100fb.h
+++ b/drivers/video/sa1100fb.h
@@ -100,7 +100,7 @@ struct sa1100fb_info {
 
 	volatile u_char		state;
 	volatile u_char		task_state;
-	struct semaphore	ctrlr_sem;
+	struct mutex		ctrlr_lock;
 	wait_queue_head_t	ctrlr_wait;
 	struct work_struct	task;
 
diff --git a/drivers/video/sh7760fb.c b/drivers/video/sh7760fb.c
new file mode 100644
index 00000000000..4d0e28c5790
--- /dev/null
+++ b/drivers/video/sh7760fb.c
@@ -0,0 +1,658 @@
+/*
+ * SH7760/SH7763 LCDC Framebuffer driver.
+ *
+ * (c) 2006-2008 MSC Vertriebsges.m.b.H.,
+ *             Manuel Lauss <mano@roarinelk.homelinux.net>
+ * (c) 2008 Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>
+ *
+ *  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.
+ *
+ * PLEASE HAVE A LOOK AT Documentation/fb/sh7760fb.txt!
+ *
+ * Thanks to Siegfried Schaefer <s.schaefer at schaefer-edv.de>
+ *     for his original source and testing!
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <asm/sh7760fb.h>
+
+struct sh7760fb_par {
+	void __iomem *base;
+	int irq;
+
+	struct sh7760fb_platdata *pd;	/* display information */
+
+	dma_addr_t fbdma;	/* physical address */
+
+	int rot;		/* rotation enabled? */
+
+	u32 pseudo_palette[16];
+
+	struct platform_device *dev;
+	struct resource *ioarea;
+	struct completion vsync;	/* vsync irq event */
+};
+
+static irqreturn_t sh7760fb_irq(int irq, void *data)
+{
+	struct completion *c = data;
+
+	complete(c);
+
+	return IRQ_HANDLED;
+}
+
+static void sh7760fb_wait_vsync(struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+
+	if (par->pd->novsync)
+		return;
+
+	iowrite16(ioread16(par->base + LDINTR) & ~VINT_CHECK,
+		  par->base + LDINTR);
+
+	if (par->irq < 0) {
+		/* poll for vert. retrace: status bit is sticky */
+		while (!(ioread16(par->base + LDINTR) & VINT_CHECK))
+			cpu_relax();
+	} else {
+		/* a "wait_for_irq_event(par->irq)" would be extremely nice */
+		init_completion(&par->vsync);
+		enable_irq(par->irq);
+		wait_for_completion(&par->vsync);
+		disable_irq_nosync(par->irq);
+	}
+}
+
+/* wait_for_lps - wait until power supply has reached a certain state. */
+static int wait_for_lps(struct sh7760fb_par *par, int val)
+{
+	int i = 100;
+	while (--i && ((ioread16(par->base + LDPMMR) & 3) != val))
+		msleep(1);
+
+	if (i <= 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+/* en/disable the LCDC */
+static int sh7760fb_blank(int blank, struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+	struct sh7760fb_platdata *pd = par->pd;
+	unsigned short cntr = ioread16(par->base + LDCNTR);
+	unsigned short intr = ioread16(par->base + LDINTR);
+	int lps;
+
+	if (blank == FB_BLANK_UNBLANK) {
+		intr |= VINT_START;
+		cntr = LDCNTR_DON2 | LDCNTR_DON;
+		lps = 3;
+	} else {
+		intr &= ~VINT_START;
+		cntr = LDCNTR_DON2;
+		lps = 0;
+	}
+
+	if (pd->blank)
+		pd->blank(blank);
+
+	iowrite16(intr, par->base + LDINTR);
+	iowrite16(cntr, par->base + LDCNTR);
+
+	return wait_for_lps(par, lps);
+}
+
+/* set color registers */
+static int sh7760fb_setcmap(struct fb_cmap *cmap, struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+	u32 s = cmap->start;
+	u32 l = cmap->len;
+	u16 *r = cmap->red;
+	u16 *g = cmap->green;
+	u16 *b = cmap->blue;
+	u32 col, tmo;
+	int ret;
+
+	ret = 0;
+
+	sh7760fb_wait_vsync(info);
+
+	/* request palette access */
+	iowrite16(LDPALCR_PALEN, par->base + LDPALCR);
+
+	/* poll for access grant */
+	tmo = 100;
+	while (!(ioread16(par->base + LDPALCR) & LDPALCR_PALS) && (--tmo))
+		cpu_relax();
+
+	if (!tmo) {
+		ret = 1;
+		dev_dbg(info->dev, "no palette access!\n");
+		goto out;
+	}
+
+	while (l && (s < 256)) {
+		col = ((*r) & 0xff) << 16;
+		col |= ((*g) & 0xff) << 8;
+		col |= ((*b) & 0xff);
+		col &= SH7760FB_PALETTE_MASK;
+
+		if (s < 16)
+			((u32 *) (info->pseudo_palette))[s] = s;
+
+		s++;
+		l--;
+		r++;
+		g++;
+		b++;
+	}
+out:
+	iowrite16(0, par->base + LDPALCR);
+	return ret;
+}
+
+static void encode_fix(struct fb_fix_screeninfo *fix, struct fb_info *info,
+		       unsigned long stride)
+{
+	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+	strcpy(fix->id, "sh7760-lcdc");
+
+	fix->smem_start = (unsigned long)info->screen_base;
+	fix->smem_len = info->screen_size;
+
+	fix->line_length = stride;
+}
+
+static int sh7760fb_get_color_info(struct device *dev,
+				   u16 lddfr, int *bpp, int *gray)
+{
+	int lbpp, lgray;
+
+	lgray = lbpp = 0;
+
+	switch (lddfr & LDDFR_COLOR_MASK) {
+	case LDDFR_1BPP_MONO:
+		lgray = 1;
+		lbpp = 1;
+		break;
+	case LDDFR_2BPP_MONO:
+		lgray = 1;
+		lbpp = 2;
+		break;
+	case LDDFR_4BPP_MONO:
+		lgray = 1;
+	case LDDFR_4BPP:
+		lbpp = 4;
+		break;
+	case LDDFR_6BPP_MONO:
+		lgray = 1;
+	case LDDFR_8BPP:
+		lbpp = 8;
+		break;
+	case LDDFR_16BPP_RGB555:
+	case LDDFR_16BPP_RGB565:
+		lbpp = 16;
+		lgray = 0;
+		break;
+	default:
+		dev_dbg(dev, "unsupported LDDFR bit depth.\n");
+		return -EINVAL;
+	}
+
+	if (bpp)
+		*bpp = lbpp;
+	if (gray)
+		*gray = lgray;
+
+	return 0;
+}
+
+static int sh7760fb_check_var(struct fb_var_screeninfo *var,
+			      struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct sh7760fb_par *par = info->par;
+	int ret, bpp;
+
+	/* get color info from register value */
+	ret = sh7760fb_get_color_info(info->dev, par->pd->lddfr, &bpp, NULL);
+	if (ret)
+		return ret;
+
+	var->bits_per_pixel = bpp;
+
+	if ((var->grayscale) && (var->bits_per_pixel == 1))
+		fix->visual = FB_VISUAL_MONO10;
+	else if (var->bits_per_pixel >= 15)
+		fix->visual = FB_VISUAL_TRUECOLOR;
+	else
+		fix->visual = FB_VISUAL_PSEUDOCOLOR;
+
+	/* TODO: add some more validation here */
+	return 0;
+}
+
+/*
+ * sh7760fb_set_par - set videomode.
+ *
+ * NOTE: The rotation, grayscale and DSTN codepaths are
+ *     totally untested!
+ */
+static int sh7760fb_set_par(struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+	struct fb_videomode *vm = par->pd->def_mode;
+	unsigned long sbase, dstn_off, ldsarl, stride;
+	unsigned short hsynp, hsynw, htcn, hdcn;
+	unsigned short vsynp, vsynw, vtln, vdln;
+	unsigned short lddfr, ldmtr;
+	int ret, bpp, gray;
+
+	par->rot = par->pd->rotate;
+
+	/* rotate only works with xres <= 320 */
+	if (par->rot && (vm->xres > 320)) {
+		dev_dbg(info->dev, "rotation disabled due to display size\n");
+		par->rot = 0;
+	}
+
+	/* calculate LCDC reg vals from display parameters */
+	hsynp = vm->right_margin + vm->xres;
+	hsynw = vm->hsync_len;
+	htcn = vm->left_margin + hsynp + hsynw;
+	hdcn = vm->xres;
+	vsynp = vm->lower_margin + vm->yres;
+	vsynw = vm->vsync_len;
+	vtln = vm->upper_margin + vsynp + vsynw;
+	vdln = vm->yres;
+
+	/* get color info from register value */
+	ret = sh7760fb_get_color_info(info->dev, par->pd->lddfr, &bpp, &gray);
+	if (ret)
+		return ret;
+
+	dev_dbg(info->dev, "%dx%d %dbpp %s (orientation %s)\n", hdcn,
+		vdln, bpp, gray ? "grayscale" : "color",
+		par->rot ? "rotated" : "normal");
+
+#ifdef CONFIG_CPU_LITTLE_ENDIAN
+	lddfr = par->pd->lddfr | (1 << 8);
+#else
+	lddfr = par->pd->lddfr & ~(1 << 8);
+#endif
+
+	ldmtr = par->pd->ldmtr;
+
+	if (!(vm->sync & FB_SYNC_HOR_HIGH_ACT))
+		ldmtr |= LDMTR_CL1POL;
+	if (!(vm->sync & FB_SYNC_VERT_HIGH_ACT))
+		ldmtr |= LDMTR_FLMPOL;
+
+	/* shut down LCDC before changing display parameters */
+	sh7760fb_blank(FB_BLANK_POWERDOWN, info);
+
+	iowrite16(par->pd->ldickr, par->base + LDICKR);	/* pixclock */
+	iowrite16(ldmtr, par->base + LDMTR);	/* polarities */
+	iowrite16(lddfr, par->base + LDDFR);	/* color/depth */
+	iowrite16((par->rot ? 1 << 13 : 0), par->base + LDSMR);	/* rotate */
+	iowrite16(par->pd->ldpmmr, par->base + LDPMMR);	/* Power Management */
+	iowrite16(par->pd->ldpspr, par->base + LDPSPR);	/* Power Supply Ctrl */
+
+	/* display resolution */
+	iowrite16(((htcn >> 3) - 1) | (((hdcn >> 3) - 1) << 8),
+		  par->base + LDHCNR);
+	iowrite16(vdln - 1, par->base + LDVDLNR);
+	iowrite16(vtln - 1, par->base + LDVTLNR);
+	/* h/v sync signals */
+	iowrite16((vsynp - 1) | ((vsynw - 1) << 12), par->base + LDVSYNR);
+	iowrite16(((hsynp >> 3) - 1) | (((hsynw >> 3) - 1) << 12),
+		  par->base + LDHSYNR);
+	/* AC modulation sig */
+	iowrite16(par->pd->ldaclnr, par->base + LDACLNR);
+
+	stride = (par->rot) ? vtln : hdcn;
+	if (!gray)
+		stride *= (bpp + 7) >> 3;
+	else {
+		if (bpp == 1)
+			stride >>= 3;
+		else if (bpp == 2)
+			stride >>= 2;
+		else if (bpp == 4)
+			stride >>= 1;
+		/* 6 bpp == 8 bpp */
+	}
+
+	/* if rotated, stride must be power of 2 */
+	if (par->rot) {
+		unsigned long bit = 1 << 31;
+		while (bit) {
+			if (stride & bit)
+				break;
+			bit >>= 1;
+		}
+		if (stride & ~bit)
+			stride = bit << 1;	/* not P-o-2, round up */
+	}
+	iowrite16(stride, par->base + LDLAOR);
+
+	/* set display mem start address */
+	sbase = (unsigned long)par->fbdma;
+	if (par->rot)
+		sbase += (hdcn - 1) * stride;
+
+	iowrite32(sbase, par->base + LDSARU);
+
+	/*
+	 * for DSTN need to set address for lower half.
+	 * I (mlau) don't know which address to set it to,
+	 * so I guessed at (stride * yres/2).
+	 */
+	if (((ldmtr & 0x003f) >= LDMTR_DSTN_MONO_8) &&
+	    ((ldmtr & 0x003f) <= LDMTR_DSTN_COLOR_16)) {
+
+		dev_dbg(info->dev, " ***** DSTN untested! *****\n");
+
+		dstn_off = stride;
+		if (par->rot)
+			dstn_off *= hdcn >> 1;
+		else
+			dstn_off *= vdln >> 1;
+
+		ldsarl = sbase + dstn_off;
+	} else
+		ldsarl = 0;
+
+	iowrite32(ldsarl, par->base + LDSARL);	/* mem for lower half of DSTN */
+
+	encode_fix(&info->fix, info, stride);
+	sh7760fb_check_var(&info->var, info);
+
+	sh7760fb_blank(FB_BLANK_UNBLANK, info);	/* panel on! */
+
+	dev_dbg(info->dev, "hdcn  : %6d htcn  : %6d\n", hdcn, htcn);
+	dev_dbg(info->dev, "hsynw : %6d hsynp : %6d\n", hsynw, hsynp);
+	dev_dbg(info->dev, "vdln  : %6d vtln  : %6d\n", vdln, vtln);
+	dev_dbg(info->dev, "vsynw : %6d vsynp : %6d\n", vsynw, vsynp);
+	dev_dbg(info->dev, "clksrc: %6d clkdiv: %6d\n",
+		(par->pd->ldickr >> 12) & 3, par->pd->ldickr & 0x1f);
+	dev_dbg(info->dev, "ldpmmr: 0x%04x ldpspr: 0x%04x\n", par->pd->ldpmmr,
+		par->pd->ldpspr);
+	dev_dbg(info->dev, "ldmtr : 0x%04x lddfr : 0x%04x\n", ldmtr, lddfr);
+	dev_dbg(info->dev, "ldlaor: %ld\n", stride);
+	dev_dbg(info->dev, "ldsaru: 0x%08lx ldsarl: 0x%08lx\n", sbase, ldsarl);
+
+	return 0;
+}
+
+static struct fb_ops sh7760fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_blank = sh7760fb_blank,
+	.fb_check_var = sh7760fb_check_var,
+	.fb_setcmap = sh7760fb_setcmap,
+	.fb_set_par = sh7760fb_set_par,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static void sh7760fb_free_mem(struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+
+	if (!info->screen_base)
+		return;
+
+	dma_free_coherent(info->dev, info->screen_size,
+			  info->screen_base, par->fbdma);
+
+	par->fbdma = 0;
+	info->screen_base = NULL;
+	info->screen_size = 0;
+}
+
+/* allocate the framebuffer memory. This memory must be in Area3,
+ * (dictated by the DMA engine) and contiguous, at a 512 byte boundary.
+ */
+static int sh7760fb_alloc_mem(struct fb_info *info)
+{
+	struct sh7760fb_par *par = info->par;
+	void *fbmem;
+	unsigned long vram;
+	int ret, bpp;
+
+	if (info->screen_base)
+		return 0;
+
+	/* get color info from register value */
+	ret = sh7760fb_get_color_info(info->dev, par->pd->lddfr, &bpp, NULL);
+	if (ret) {
+		printk(KERN_ERR "colinfo\n");
+		return ret;
+	}
+
+	/* min VRAM: xres_min = 16, yres_min = 1, bpp = 1: 2byte -> 1 page
+	   max VRAM: xres_max = 1024, yres_max = 1024, bpp = 16: 2MB */
+
+	vram = info->var.xres * info->var.yres;
+	if (info->var.grayscale) {
+		if (bpp == 1)
+			vram >>= 3;
+		else if (bpp == 2)
+			vram >>= 2;
+		else if (bpp == 4)
+			vram >>= 1;
+	} else if (bpp > 8)
+		vram *= 2;
+	if ((vram < 1) || (vram > 1024 * 2048)) {
+		dev_dbg(info->dev, "too much VRAM required. Check settings\n");
+		return -ENODEV;
+	}
+
+	if (vram < PAGE_SIZE)
+		vram = PAGE_SIZE;
+
+	fbmem = dma_alloc_coherent(info->dev, vram, &par->fbdma, GFP_KERNEL);
+
+	if (!fbmem)
+		return -ENOMEM;
+
+	if ((par->fbdma & SH7760FB_DMA_MASK) != SH7760FB_DMA_MASK) {
+		sh7760fb_free_mem(info);
+		dev_err(info->dev, "kernel gave me memory at 0x%08lx, which is"
+			"unusable for the LCDC\n", (unsigned long)par->fbdma);
+		return -ENOMEM;
+	}
+
+	info->screen_base = fbmem;
+	info->screen_size = vram;
+
+	return 0;
+}
+
+static int __devinit sh7760fb_probe(struct platform_device *pdev)
+{
+	struct fb_info *info;
+	struct resource *res;
+	struct sh7760fb_par *par;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(res == NULL)) {
+		dev_err(&pdev->dev, "invalid resource\n");
+		return -EINVAL;
+	}
+
+	info = framebuffer_alloc(sizeof(struct sh7760fb_par), &pdev->dev);
+	if (!info)
+		return -ENOMEM;
+
+	par = info->par;
+	par->dev = pdev;
+
+	par->pd = pdev->dev.platform_data;
+	if (!par->pd) {
+		dev_dbg(info->dev, "no display setup data!\n");
+		ret = -ENODEV;
+		goto out_fb;
+	}
+
+	par->ioarea = request_mem_region(res->start,
+					 (res->end - res->start), pdev->name);
+	if (!par->ioarea) {
+		dev_err(&pdev->dev, "mmio area busy\n");
+		ret = -EBUSY;
+		goto out_fb;
+	}
+
+	par->base = ioremap_nocache(res->start, res->end - res->start + 1);
+	if (!par->base) {
+		dev_err(&pdev->dev, "cannot remap\n");
+		ret = -ENODEV;
+		goto out_res;
+	}
+
+	iowrite16(0, par->base + LDINTR);	/* disable vsync irq */
+	par->irq = platform_get_irq(pdev, 0);
+	if (par->irq >= 0) {
+		ret = request_irq(par->irq, sh7760fb_irq, 0,
+				  "sh7760-lcdc", &par->vsync);
+		if (ret) {
+			dev_err(&pdev->dev, "cannot grab IRQ\n");
+			par->irq = -ENXIO;
+		} else
+			disable_irq_nosync(par->irq);
+	}
+
+	fb_videomode_to_var(&info->var, par->pd->def_mode);
+
+	ret = sh7760fb_alloc_mem(info);
+	if (ret) {
+		dev_dbg(info->dev, "framebuffer memory allocation failed!\n");
+		goto out_unmap;
+	}
+
+	info->pseudo_palette = par->pseudo_palette;
+
+	/* fixup color register bitpositions. These are fixed by hardware */
+	info->var.red.offset = 11;
+	info->var.red.length = 5;
+	info->var.red.msb_right = 0;
+
+	info->var.green.offset = 5;
+	info->var.green.length = 6;
+	info->var.green.msb_right = 0;
+
+	info->var.blue.offset = 0;
+	info->var.blue.length = 5;
+	info->var.blue.msb_right = 0;
+
+	info->var.transp.offset = 0;
+	info->var.transp.length = 0;
+	info->var.transp.msb_right = 0;
+
+	/* set the DON2 bit now, before cmap allocation, as it will randomize
+	 * palette memory.
+	 */
+	iowrite16(LDCNTR_DON2, par->base + LDCNTR);
+	info->fbops = &sh7760fb_ops;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		dev_dbg(info->dev, "Unable to allocate cmap memory\n");
+		goto out_mem;
+	}
+
+	ret = register_framebuffer(info);
+	if (ret < 0) {
+		dev_dbg(info->dev, "cannot register fb!\n");
+		goto out_cmap;
+	}
+	platform_set_drvdata(pdev, info);
+
+	printk(KERN_INFO "%s: memory at phys 0x%08lx-0x%08lx, size %ld KiB\n",
+	       pdev->name,
+	       (unsigned long)par->fbdma,
+	       (unsigned long)(par->fbdma + info->screen_size - 1),
+	       info->screen_size >> 10);
+
+	return 0;
+
+out_cmap:
+	sh7760fb_blank(FB_BLANK_POWERDOWN, info);
+	fb_dealloc_cmap(&info->cmap);
+out_mem:
+	sh7760fb_free_mem(info);
+out_unmap:
+	if (par->irq >= 0)
+		free_irq(par->irq, &par->vsync);
+	iounmap(par->base);
+out_res:
+	release_resource(par->ioarea);
+	kfree(par->ioarea);
+out_fb:
+	framebuffer_release(info);
+	return ret;
+}
+
+static int __devexit sh7760fb_remove(struct platform_device *dev)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+	struct sh7760fb_par *par = info->par;
+
+	sh7760fb_blank(FB_BLANK_POWERDOWN, info);
+	unregister_framebuffer(info);
+	fb_dealloc_cmap(&info->cmap);
+	sh7760fb_free_mem(info);
+	if (par->irq >= 0)
+		free_irq(par->irq, par);
+	iounmap(par->base);
+	release_resource(par->ioarea);
+	kfree(par->ioarea);
+	framebuffer_release(info);
+	platform_set_drvdata(dev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver sh7760_lcdc_driver = {
+	.driver = {
+		   .name = "sh7760-lcdc",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = sh7760fb_probe,
+	.remove = __devexit_p(sh7760fb_remove),
+};
+
+static int __init sh7760fb_init(void)
+{
+	return platform_driver_register(&sh7760_lcdc_driver);
+}
+
+static void __exit sh7760fb_exit(void)
+{
+	platform_driver_unregister(&sh7760_lcdc_driver);
+}
+
+module_init(sh7760fb_init);
+module_exit(sh7760fb_exit);
+
+MODULE_AUTHOR("Nobuhiro Iwamatsu, Manuel Lauss");
+MODULE_DESCRIPTION("FBdev for SH7760/63 integrated LCD Controller");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
new file mode 100644
index 00000000000..f6ef6cca73c
--- /dev/null
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -0,0 +1,725 @@
+/*
+ * SuperH Mobile LCDC Framebuffer
+ *
+ * Copyright (c) 2008 Magnus Damm
+ *
+ * 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/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <asm/sh_mobile_lcdc.h>
+
+#define PALETTE_NR 16
+
+struct sh_mobile_lcdc_priv;
+struct sh_mobile_lcdc_chan {
+	struct sh_mobile_lcdc_priv *lcdc;
+	unsigned long *reg_offs;
+	unsigned long ldmt1r_value;
+	unsigned long enabled; /* ME and SE in LDCNT2R */
+	struct sh_mobile_lcdc_chan_cfg cfg;
+	u32 pseudo_palette[PALETTE_NR];
+	struct fb_info info;
+	dma_addr_t dma_handle;
+};
+
+struct sh_mobile_lcdc_priv {
+	void __iomem *base;
+	struct clk *clk;
+	unsigned long lddckr;
+	struct sh_mobile_lcdc_chan ch[2];
+};
+
+/* shared registers */
+#define _LDDCKR 0x410
+#define _LDDCKSTPR 0x414
+#define _LDINTR 0x468
+#define _LDSR 0x46c
+#define _LDCNT1R 0x470
+#define _LDCNT2R 0x474
+#define _LDDDSR 0x47c
+#define _LDDWD0R 0x800
+#define _LDDRDR 0x840
+#define _LDDWAR 0x900
+#define _LDDRAR 0x904
+
+/* per-channel registers */
+enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
+       LDSA1R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR };
+
+static unsigned long lcdc_offs_mainlcd[] = {
+	[LDDCKPAT1R] = 0x400,
+	[LDDCKPAT2R] = 0x404,
+	[LDMT1R] = 0x418,
+	[LDMT2R] = 0x41c,
+	[LDMT3R] = 0x420,
+	[LDDFR] = 0x424,
+	[LDSM1R] = 0x428,
+	[LDSA1R] = 0x430,
+	[LDMLSR] = 0x438,
+	[LDHCNR] = 0x448,
+	[LDHSYNR] = 0x44c,
+	[LDVLNR] = 0x450,
+	[LDVSYNR] = 0x454,
+	[LDPMR] = 0x460,
+};
+
+static unsigned long lcdc_offs_sublcd[] = {
+	[LDDCKPAT1R] = 0x408,
+	[LDDCKPAT2R] = 0x40c,
+	[LDMT1R] = 0x600,
+	[LDMT2R] = 0x604,
+	[LDMT3R] = 0x608,
+	[LDDFR] = 0x60c,
+	[LDSM1R] = 0x610,
+	[LDSA1R] = 0x618,
+	[LDMLSR] = 0x620,
+	[LDHCNR] = 0x624,
+	[LDHSYNR] = 0x628,
+	[LDVLNR] = 0x62c,
+	[LDVSYNR] = 0x630,
+	[LDPMR] = 0x63c,
+};
+
+#define START_LCDC	0x00000001
+#define LCDC_RESET	0x00000100
+#define DISPLAY_BEU	0x00000008
+#define LCDC_ENABLE	0x00000001
+
+static void lcdc_write_chan(struct sh_mobile_lcdc_chan *chan,
+			    int reg_nr, unsigned long data)
+{
+	iowrite32(data, chan->lcdc->base + chan->reg_offs[reg_nr]);
+}
+
+static unsigned long lcdc_read_chan(struct sh_mobile_lcdc_chan *chan,
+				    int reg_nr)
+{
+	return ioread32(chan->lcdc->base + chan->reg_offs[reg_nr]);
+}
+
+static void lcdc_write(struct sh_mobile_lcdc_priv *priv,
+		       unsigned long reg_offs, unsigned long data)
+{
+	iowrite32(data, priv->base + reg_offs);
+}
+
+static unsigned long lcdc_read(struct sh_mobile_lcdc_priv *priv,
+			       unsigned long reg_offs)
+{
+	return ioread32(priv->base + reg_offs);
+}
+
+static void lcdc_wait_bit(struct sh_mobile_lcdc_priv *priv,
+			  unsigned long reg_offs,
+			  unsigned long mask, unsigned long until)
+{
+	while ((lcdc_read(priv, reg_offs) & mask) != until)
+		cpu_relax();
+}
+
+static int lcdc_chan_is_sublcd(struct sh_mobile_lcdc_chan *chan)
+{
+	return chan->cfg.chan == LCDC_CHAN_SUBLCD;
+}
+
+static void lcdc_sys_write_index(void *handle, unsigned long data)
+{
+	struct sh_mobile_lcdc_chan *ch = handle;
+
+	lcdc_write(ch->lcdc, _LDDWD0R, data | 0x10000000);
+	lcdc_wait_bit(ch->lcdc, _LDSR, 2, 0);
+	lcdc_write(ch->lcdc, _LDDWAR, 1 | (lcdc_chan_is_sublcd(ch) ? 2 : 0));
+}
+
+static void lcdc_sys_write_data(void *handle, unsigned long data)
+{
+	struct sh_mobile_lcdc_chan *ch = handle;
+
+	lcdc_write(ch->lcdc, _LDDWD0R, data | 0x11000000);
+	lcdc_wait_bit(ch->lcdc, _LDSR, 2, 0);
+	lcdc_write(ch->lcdc, _LDDWAR, 1 | (lcdc_chan_is_sublcd(ch) ? 2 : 0));
+}
+
+static unsigned long lcdc_sys_read_data(void *handle)
+{
+	struct sh_mobile_lcdc_chan *ch = handle;
+
+	lcdc_write(ch->lcdc, _LDDRDR, 0x01000000);
+	lcdc_wait_bit(ch->lcdc, _LDSR, 2, 0);
+	lcdc_write(ch->lcdc, _LDDRAR, 1 | (lcdc_chan_is_sublcd(ch) ? 2 : 0));
+	udelay(1);
+
+	return lcdc_read(ch->lcdc, _LDDRDR) & 0xffff;
+}
+
+struct sh_mobile_lcdc_sys_bus_ops sh_mobile_lcdc_sys_bus_ops = {
+	lcdc_sys_write_index,
+	lcdc_sys_write_data,
+	lcdc_sys_read_data,
+};
+
+static void sh_mobile_lcdc_start_stop(struct sh_mobile_lcdc_priv *priv,
+				      int start)
+{
+	unsigned long tmp = lcdc_read(priv, _LDCNT2R);
+	int k;
+
+	/* start or stop the lcdc */
+	if (start)
+		lcdc_write(priv, _LDCNT2R, tmp | START_LCDC);
+	else
+		lcdc_write(priv, _LDCNT2R, tmp & ~START_LCDC);
+
+	/* wait until power is applied/stopped on all channels */
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++)
+		if (lcdc_read(priv, _LDCNT2R) & priv->ch[k].enabled)
+			while (1) {
+				tmp = lcdc_read_chan(&priv->ch[k], LDPMR) & 3;
+				if (start && tmp == 3)
+					break;
+				if (!start && tmp == 0)
+					break;
+				cpu_relax();
+			}
+
+	if (!start)
+		lcdc_write(priv, _LDDCKSTPR, 1); /* stop dotclock */
+}
+
+static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
+{
+	struct sh_mobile_lcdc_chan *ch;
+	struct fb_videomode *lcd_cfg;
+	struct sh_mobile_lcdc_board_cfg	*board_cfg;
+	unsigned long tmp;
+	int k, m;
+	int ret = 0;
+
+	/* reset */
+	lcdc_write(priv, _LDCNT2R, lcdc_read(priv, _LDCNT2R) | LCDC_RESET);
+	lcdc_wait_bit(priv, _LDCNT2R, LCDC_RESET, 0);
+
+	/* enable LCDC channels */
+	tmp = lcdc_read(priv, _LDCNT2R);
+	tmp |= priv->ch[0].enabled;
+	tmp |= priv->ch[1].enabled;
+	lcdc_write(priv, _LDCNT2R, tmp);
+
+	/* read data from external memory, avoid using the BEU for now */
+	lcdc_write(priv, _LDCNT2R, lcdc_read(priv, _LDCNT2R) & ~DISPLAY_BEU);
+
+	/* stop the lcdc first */
+	sh_mobile_lcdc_start_stop(priv, 0);
+
+	/* configure clocks */
+	tmp = priv->lddckr;
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		ch = &priv->ch[k];
+
+		if (!priv->ch[k].enabled)
+			continue;
+
+		m = ch->cfg.clock_divider;
+		if (!m)
+			continue;
+
+		if (m == 1)
+			m = 1 << 6;
+		tmp |= m << (lcdc_chan_is_sublcd(ch) ? 8 : 0);
+
+		lcdc_write_chan(ch, LDDCKPAT1R, 0x00000000);
+		lcdc_write_chan(ch, LDDCKPAT2R, (1 << (m/2)) - 1);
+	}
+
+	lcdc_write(priv, _LDDCKR, tmp);
+
+	/* start dotclock again */
+	lcdc_write(priv, _LDDCKSTPR, 0);
+	lcdc_wait_bit(priv, _LDDCKSTPR, ~0, 0);
+
+	/* interrupts are disabled */
+	lcdc_write(priv, _LDINTR, 0);
+
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		ch = &priv->ch[k];
+		lcd_cfg = &ch->cfg.lcd_cfg;
+
+		if (!ch->enabled)
+			continue;
+
+		tmp = ch->ldmt1r_value;
+		tmp |= (lcd_cfg->sync & FB_SYNC_VERT_HIGH_ACT) ? 0 : 1 << 28;
+		tmp |= (lcd_cfg->sync & FB_SYNC_HOR_HIGH_ACT) ? 0 : 1 << 27;
+		lcdc_write_chan(ch, LDMT1R, tmp);
+
+		/* setup SYS bus */
+		lcdc_write_chan(ch, LDMT2R, ch->cfg.sys_bus_cfg.ldmt2r);
+		lcdc_write_chan(ch, LDMT3R, ch->cfg.sys_bus_cfg.ldmt3r);
+
+		/* horizontal configuration */
+		tmp = lcd_cfg->xres + lcd_cfg->hsync_len;
+		tmp += lcd_cfg->left_margin;
+		tmp += lcd_cfg->right_margin;
+		tmp /= 8; /* HTCN */
+		tmp |= (lcd_cfg->xres / 8) << 16; /* HDCN */
+		lcdc_write_chan(ch, LDHCNR, tmp);
+
+		tmp = lcd_cfg->xres;
+		tmp += lcd_cfg->right_margin;
+		tmp /= 8; /* HSYNP */
+		tmp |= (lcd_cfg->hsync_len / 8) << 16; /* HSYNW */
+		lcdc_write_chan(ch, LDHSYNR, tmp);
+
+		/* power supply */
+		lcdc_write_chan(ch, LDPMR, 0);
+
+		/* vertical configuration */
+		tmp = lcd_cfg->yres + lcd_cfg->vsync_len;
+		tmp += lcd_cfg->upper_margin;
+		tmp += lcd_cfg->lower_margin; /* VTLN */
+		tmp |= lcd_cfg->yres << 16; /* VDLN */
+		lcdc_write_chan(ch, LDVLNR, tmp);
+
+		tmp = lcd_cfg->yres;
+		tmp += lcd_cfg->lower_margin; /* VSYNP */
+		tmp |= lcd_cfg->vsync_len << 16; /* VSYNW */
+		lcdc_write_chan(ch, LDVSYNR, tmp);
+
+		board_cfg = &ch->cfg.board_cfg;
+		if (board_cfg->setup_sys)
+			ret = board_cfg->setup_sys(board_cfg->board_data, ch,
+						   &sh_mobile_lcdc_sys_bus_ops);
+		if (ret)
+			return ret;
+	}
+
+	/* --- display_lcdc_data() --- */
+	lcdc_write(priv, _LDINTR, 0x00000f00);
+
+	/* word and long word swap */
+	lcdc_write(priv, _LDDDSR, lcdc_read(priv, _LDDDSR) | 6);
+
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		ch = &priv->ch[k];
+
+		if (!priv->ch[k].enabled)
+			continue;
+
+		/* set bpp format in PKF[4:0] */
+		tmp = lcdc_read_chan(ch, LDDFR);
+		tmp &= ~(0x0001001f);
+		tmp |= (priv->ch[k].info.var.bits_per_pixel == 16) ? 3 : 0;
+		lcdc_write_chan(ch, LDDFR, tmp);
+
+		/* point out our frame buffer */
+		lcdc_write_chan(ch, LDSA1R, ch->info.fix.smem_start);
+
+		/* set line size */
+		lcdc_write_chan(ch, LDMLSR, ch->info.fix.line_length);
+
+		/* continuous read mode */
+		lcdc_write_chan(ch, LDSM1R, 0);
+	}
+
+	/* display output */
+	lcdc_write(priv, _LDCNT1R, LCDC_ENABLE);
+
+	/* start the lcdc */
+	sh_mobile_lcdc_start_stop(priv, 1);
+
+	/* tell the board code to enable the panel */
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		ch = &priv->ch[k];
+		board_cfg = &ch->cfg.board_cfg;
+		if (board_cfg->display_on)
+			board_cfg->display_on(board_cfg->board_data);
+	}
+
+	return 0;
+}
+
+static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
+{
+	struct sh_mobile_lcdc_chan *ch;
+	struct sh_mobile_lcdc_board_cfg	*board_cfg;
+	int k;
+
+	/* tell the board code to disable the panel */
+	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		ch = &priv->ch[k];
+		board_cfg = &ch->cfg.board_cfg;
+		if (board_cfg->display_off)
+			board_cfg->display_off(board_cfg->board_data);
+	}
+
+	/* stop the lcdc */
+	sh_mobile_lcdc_start_stop(priv, 0);
+}
+
+static int sh_mobile_lcdc_check_interface(struct sh_mobile_lcdc_chan *ch)
+{
+	int ifm, miftyp;
+
+	switch (ch->cfg.interface_type) {
+	case RGB8: ifm = 0; miftyp = 0; break;
+	case RGB9: ifm = 0; miftyp = 4; break;
+	case RGB12A: ifm = 0; miftyp = 5; break;
+	case RGB12B: ifm = 0; miftyp = 6; break;
+	case RGB16: ifm = 0; miftyp = 7; break;
+	case RGB18: ifm = 0; miftyp = 10; break;
+	case RGB24: ifm = 0; miftyp = 11; break;
+	case SYS8A: ifm = 1; miftyp = 0; break;
+	case SYS8B: ifm = 1; miftyp = 1; break;
+	case SYS8C: ifm = 1; miftyp = 2; break;
+	case SYS8D: ifm = 1; miftyp = 3; break;
+	case SYS9: ifm = 1; miftyp = 4; break;
+	case SYS12: ifm = 1; miftyp = 5; break;
+	case SYS16A: ifm = 1; miftyp = 7; break;
+	case SYS16B: ifm = 1; miftyp = 8; break;
+	case SYS16C: ifm = 1; miftyp = 9; break;
+	case SYS18: ifm = 1; miftyp = 10; break;
+	case SYS24: ifm = 1; miftyp = 11; break;
+	default: goto bad;
+	}
+
+	/* SUBLCD only supports SYS interface */
+	if (lcdc_chan_is_sublcd(ch)) {
+		if (ifm == 0)
+			goto bad;
+		else
+			ifm = 0;
+	}
+
+	ch->ldmt1r_value = (ifm << 12) | miftyp;
+	return 0;
+ bad:
+	return -EINVAL;
+}
+
+static int sh_mobile_lcdc_setup_clocks(struct device *dev, int clock_source,
+				       struct sh_mobile_lcdc_priv *priv)
+{
+	char *str;
+	int icksel;
+
+	switch (clock_source) {
+	case LCDC_CLK_BUS: str = "bus_clk"; icksel = 0; break;
+	case LCDC_CLK_PERIPHERAL: str = "peripheral_clk"; icksel = 1; break;
+	case LCDC_CLK_EXTERNAL: str = NULL; icksel = 2; break;
+	default:
+		return -EINVAL;
+	}
+
+	priv->lddckr = icksel << 16;
+
+	if (str) {
+		priv->clk = clk_get(dev, str);
+		if (IS_ERR(priv->clk)) {
+			dev_err(dev, "cannot get clock %s\n", str);
+			return PTR_ERR(priv->clk);
+		}
+
+		clk_enable(priv->clk);
+	}
+
+	return 0;
+}
+
+static int sh_mobile_lcdc_setcolreg(u_int regno,
+				    u_int red, u_int green, u_int blue,
+				    u_int transp, struct fb_info *info)
+{
+	u32 *palette = info->pseudo_palette;
+
+	if (regno >= PALETTE_NR)
+		return -EINVAL;
+
+	/* only FB_VISUAL_TRUECOLOR supported */
+
+	red >>= 16 - info->var.red.length;
+	green >>= 16 - info->var.green.length;
+	blue >>= 16 - info->var.blue.length;
+	transp >>= 16 - info->var.transp.length;
+
+	palette[regno] = (red << info->var.red.offset) |
+	  (green << info->var.green.offset) |
+	  (blue << info->var.blue.offset) |
+	  (transp << info->var.transp.offset);
+
+	return 0;
+}
+
+static struct fb_fix_screeninfo sh_mobile_lcdc_fix  = {
+	.id =		"SH Mobile LCDC",
+	.type =		FB_TYPE_PACKED_PIXELS,
+	.visual =	FB_VISUAL_TRUECOLOR,
+	.accel =	FB_ACCEL_NONE,
+};
+
+static struct fb_ops sh_mobile_lcdc_ops = {
+	.fb_setcolreg	= sh_mobile_lcdc_setcolreg,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+};
+
+static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp)
+{
+	switch (bpp) {
+	case 16: /* PKF[4:0] = 00011 - RGB 565 */
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+
+	case 32: /* PKF[4:0] = 00000 - RGB 888
+		  * sh7722 pdf says 00RRGGBB but reality is GGBB00RR
+		  * this may be because LDDDSR has word swap enabled..
+		  */
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 24;
+		var->green.length = 8;
+		var->blue.offset = 16;
+		var->blue.length = 8;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	var->bits_per_pixel = bpp;
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.msb_right = 0;
+	return 0;
+}
+
+static int sh_mobile_lcdc_remove(struct platform_device *pdev);
+
+static int __init sh_mobile_lcdc_probe(struct platform_device *pdev)
+{
+	struct fb_info *info;
+	struct sh_mobile_lcdc_priv *priv;
+	struct sh_mobile_lcdc_info *pdata;
+	struct sh_mobile_lcdc_chan_cfg *cfg;
+	struct resource *res;
+	int error;
+	void *buf;
+	int i, j;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		error = -EINVAL;
+		goto err0;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "cannot find IO resource\n");
+		error = -ENOENT;
+		goto err0;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "cannot allocate device data\n");
+		error = -ENOMEM;
+		goto err0;
+	}
+
+	platform_set_drvdata(pdev, priv);
+	pdata = pdev->dev.platform_data;
+
+	j = 0;
+	for (i = 0; i < ARRAY_SIZE(pdata->ch); i++) {
+		priv->ch[j].lcdc = priv;
+		memcpy(&priv->ch[j].cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
+
+		error = sh_mobile_lcdc_check_interface(&priv->ch[i]);
+		if (error) {
+			dev_err(&pdev->dev, "unsupported interface type\n");
+			goto err1;
+		}
+
+		switch (pdata->ch[i].chan) {
+		case LCDC_CHAN_MAINLCD:
+			priv->ch[j].enabled = 1 << 1;
+			priv->ch[j].reg_offs = lcdc_offs_mainlcd;
+			j++;
+			break;
+		case LCDC_CHAN_SUBLCD:
+			priv->ch[j].enabled = 1 << 2;
+			priv->ch[j].reg_offs = lcdc_offs_sublcd;
+			j++;
+			break;
+		}
+	}
+
+	if (!j) {
+		dev_err(&pdev->dev, "no channels defined\n");
+		error = -EINVAL;
+		goto err1;
+	}
+
+	error = sh_mobile_lcdc_setup_clocks(&pdev->dev,
+					    pdata->clock_source, priv);
+	if (error) {
+		dev_err(&pdev->dev, "unable to setup clocks\n");
+		goto err1;
+	}
+
+	priv->lddckr = pdata->lddckr;
+	priv->base = ioremap_nocache(res->start, (res->end - res->start) + 1);
+
+	for (i = 0; i < j; i++) {
+		info = &priv->ch[i].info;
+		cfg = &priv->ch[i].cfg;
+
+		info->fbops = &sh_mobile_lcdc_ops;
+		info->var.xres = info->var.xres_virtual = cfg->lcd_cfg.xres;
+		info->var.yres = info->var.yres_virtual = cfg->lcd_cfg.yres;
+		info->var.activate = FB_ACTIVATE_NOW;
+		error = sh_mobile_lcdc_set_bpp(&info->var, cfg->bpp);
+		if (error)
+			break;
+
+		info->fix = sh_mobile_lcdc_fix;
+		info->fix.line_length = cfg->lcd_cfg.xres * (cfg->bpp / 8);
+		info->fix.smem_len = info->fix.line_length * cfg->lcd_cfg.yres;
+
+		buf = dma_alloc_coherent(&pdev->dev, info->fix.smem_len,
+					 &priv->ch[i].dma_handle, GFP_KERNEL);
+		if (!buf) {
+			dev_err(&pdev->dev, "unable to allocate buffer\n");
+			error = -ENOMEM;
+			break;
+		}
+
+		info->pseudo_palette = &priv->ch[i].pseudo_palette;
+		info->flags = FBINFO_FLAG_DEFAULT;
+
+		error = fb_alloc_cmap(&info->cmap, PALETTE_NR, 0);
+		if (error < 0) {
+			dev_err(&pdev->dev, "unable to allocate cmap\n");
+			dma_free_coherent(&pdev->dev, info->fix.smem_len,
+					  buf, priv->ch[i].dma_handle);
+			break;
+		}
+
+		memset(buf, 0, info->fix.smem_len);
+		info->fix.smem_start = priv->ch[i].dma_handle;
+		info->screen_base = buf;
+		info->device = &pdev->dev;
+	}
+
+	if (error)
+		goto err1;
+
+	error = sh_mobile_lcdc_start(priv);
+	if (error) {
+		dev_err(&pdev->dev, "unable to start hardware\n");
+		goto err1;
+	}
+
+	for (i = 0; i < j; i++) {
+		error = register_framebuffer(&priv->ch[i].info);
+		if (error < 0)
+			goto err1;
+	}
+
+	for (i = 0; i < j; i++) {
+		info = &priv->ch[i].info;
+		dev_info(info->dev,
+			 "registered %s/%s as %dx%d %dbpp.\n",
+			 pdev->name,
+			 (priv->ch[i].cfg.chan == LCDC_CHAN_MAINLCD) ?
+			 "mainlcd" : "sublcd",
+			 (int) priv->ch[i].cfg.lcd_cfg.xres,
+			 (int) priv->ch[i].cfg.lcd_cfg.yres,
+			 priv->ch[i].cfg.bpp);
+	}
+
+	return 0;
+ err1:
+	sh_mobile_lcdc_remove(pdev);
+ err0:
+	return error;
+}
+
+static int sh_mobile_lcdc_remove(struct platform_device *pdev)
+{
+	struct sh_mobile_lcdc_priv *priv = platform_get_drvdata(pdev);
+	struct fb_info *info;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(priv->ch); i++)
+		if (priv->ch[i].info.dev)
+			unregister_framebuffer(&priv->ch[i].info);
+
+	sh_mobile_lcdc_stop(priv);
+
+	for (i = 0; i < ARRAY_SIZE(priv->ch); i++) {
+		info = &priv->ch[i].info;
+
+		if (!info->device)
+			continue;
+
+		dma_free_coherent(&pdev->dev, info->fix.smem_len,
+				  info->screen_base, priv->ch[i].dma_handle);
+		fb_dealloc_cmap(&info->cmap);
+	}
+
+	if (priv->clk) {
+		clk_disable(priv->clk);
+		clk_put(priv->clk);
+	}
+
+	if (priv->base)
+		iounmap(priv->base);
+
+	kfree(priv);
+	return 0;
+}
+
+static struct platform_driver sh_mobile_lcdc_driver = {
+	.driver		= {
+		.name		= "sh_mobile_lcdc_fb",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= sh_mobile_lcdc_probe,
+	.remove		= sh_mobile_lcdc_remove,
+};
+
+static int __init sh_mobile_lcdc_init(void)
+{
+	return platform_driver_register(&sh_mobile_lcdc_driver);
+}
+
+static void __exit sh_mobile_lcdc_exit(void)
+{
+	platform_driver_unregister(&sh_mobile_lcdc_driver);
+}
+
+module_init(sh_mobile_lcdc_init);
+module_exit(sh_mobile_lcdc_exit);
+
+MODULE_DESCRIPTION("SuperH Mobile LCDC Framebuffer driver");
+MODULE_AUTHOR("Magnus Damm <damm@opensource.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sis/init.h b/drivers/video/sis/init.h
index f40a680df86..b96005c39c6 100644
--- a/drivers/video/sis/init.h
+++ b/drivers/video/sis/init.h
@@ -73,7 +73,6 @@
 #ifdef SIS_CP
 #undef SIS_CP
 #endif
-#include <linux/version.h>
 #include <linux/types.h>
 #include <asm/io.h>
 #include <linux/fb.h>
diff --git a/drivers/video/sis/init301.h b/drivers/video/sis/init301.h
index 7708e1e1d99..51d99222375 100644
--- a/drivers/video/sis/init301.h
+++ b/drivers/video/sis/init301.h
@@ -67,7 +67,6 @@
 #ifdef SIS_CP
 #undef SIS_CP
 #endif
-#include <linux/version.h>
 #include <linux/types.h>
 #include <asm/io.h>
 #include <linux/fb.h>
diff --git a/drivers/video/sis/initextlfb.c b/drivers/video/sis/initextlfb.c
index 47a33501549..99c04a4855d 100644
--- a/drivers/video/sis/initextlfb.c
+++ b/drivers/video/sis/initextlfb.c
@@ -30,7 +30,6 @@
 #include "vgatypes.h"
 #include "vstruct.h"
 
-#include <linux/version.h>
 #include <linux/types.h>
 #include <linux/fb.h>
 
diff --git a/drivers/video/sis/osdef.h b/drivers/video/sis/osdef.h
index c1492782cb1..6ff8f988a1a 100644
--- a/drivers/video/sis/osdef.h
+++ b/drivers/video/sis/osdef.h
@@ -87,7 +87,6 @@
 /**********************************************************************/
 
 #ifdef SIS_LINUX_KERNEL
-#include <linux/version.h>
 
 #ifdef CONFIG_FB_SIS_300
 #define SIS300
diff --git a/drivers/video/sis/sis.h b/drivers/video/sis/sis.h
index a14e8221103..7c5710e3fb5 100644
--- a/drivers/video/sis/sis.h
+++ b/drivers/video/sis/sis.h
@@ -24,8 +24,6 @@
 #ifndef _SIS_H_
 #define _SIS_H_
 
-#include <linux/version.h>
-
 #include "osdef.h"
 #include <video/sisfb.h>
 
@@ -42,16 +40,6 @@
 #define SIS_NEW_CONFIG_COMPAT
 #endif	/* CONFIG_COMPAT */
 
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8)
-#define SIS_IOTYPE1 void __iomem
-#define SIS_IOTYPE2 __iomem
-#define SISINITSTATIC static
-#else
-#define SIS_IOTYPE1 unsigned char
-#define SIS_IOTYPE2
-#define SISINITSTATIC
-#endif
-
 #undef SISFBDEBUG
 
 #ifdef SISFBDEBUG
@@ -505,8 +493,8 @@ struct sis_video_info {
 
 	unsigned long	UMAsize, LFBsize;
 
-	SIS_IOTYPE1	*video_vbase;
-	SIS_IOTYPE1	*mmio_vbase;
+	void __iomem	*video_vbase;
+	void __iomem	*mmio_vbase;
 
 	unsigned char	*bios_abase;
 
@@ -533,8 +521,8 @@ struct sis_video_info {
 	int		sisfb_nocrt2rate;
 
 	u32		heapstart;		/* offset  */
-	SIS_IOTYPE1	*sisfb_heap_start;	/* address */
-	SIS_IOTYPE1	*sisfb_heap_end;	/* address */
+	void __iomem	*sisfb_heap_start;	/* address */
+	void __iomem	*sisfb_heap_end;	/* address */
 	u32		sisfb_heap_size;
 	int		havenoheap;
 
@@ -612,7 +600,7 @@ struct sis_video_info {
 	u8		detectedpdca;
 	u8		detectedlcda;
 
-	SIS_IOTYPE1	*hwcursor_vbase;
+	void __iomem	*hwcursor_vbase;
 
 	int		chronteltype;
 	int		tvxpos, tvypos;
diff --git a/drivers/video/sis/sis_accel.c b/drivers/video/sis/sis_accel.c
index 7addf91d2fe..ceb434c95c0 100644
--- a/drivers/video/sis/sis_accel.c
+++ b/drivers/video/sis/sis_accel.c
@@ -28,7 +28,6 @@
  *			for more information and updates)
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/fb.h>
diff --git a/drivers/video/sis/sis_main.c b/drivers/video/sis/sis_main.c
index b9343844cd1..346d6458cf7 100644
--- a/drivers/video/sis/sis_main.c
+++ b/drivers/video/sis/sis_main.c
@@ -33,7 +33,6 @@
  *
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
@@ -41,13 +40,7 @@
 #include <linux/errno.h>
 #include <linux/string.h>
 #include <linux/mm.h>
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-#include <linux/tty.h>
-#else
 #include <linux/screen_info.h>
-#endif
-
 #include <linux/slab.h>
 #include <linux/fb.h>
 #include <linux/selection.h>
@@ -1167,11 +1160,7 @@ sisfb_set_mode(struct sis_video_info *ivideo, int clrscrn)
 	unsigned short modeno = ivideo->mode_no;
 
 	/* >=2.6.12's fbcon clears the screen anyway */
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,12)
-	if(!clrscrn) modeno |= 0x80;
-#else
 	modeno |= 0x80;
-#endif
 
 	outSISIDXREG(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
 
@@ -1436,11 +1425,8 @@ sisfb_set_par(struct fb_info *info)
 	if((err = sisfb_do_set_var(&info->var, 1, info)))
 		return err;
 
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10)
-	sisfb_get_fix(&info->fix, info->currcon, info);
-#else
 	sisfb_get_fix(&info->fix, -1, info);
-#endif
+
 	return 0;
 }
 
@@ -1676,14 +1662,8 @@ sisfb_blank(int blank, struct fb_info *info)
 
 /* ----------- FBDev related routines for all series ---------- */
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15)
 static int	sisfb_ioctl(struct fb_info *info, unsigned int cmd,
 			    unsigned long arg)
-#else
-static int	sisfb_ioctl(struct inode *inode, struct file *file,
-				unsigned int cmd, unsigned long arg,
-				struct fb_info *info)
-#endif
 {
 	struct sis_video_info	*ivideo = (struct sis_video_info *)info->par;
 	struct sis_memreq	sismemreq;
@@ -3986,8 +3966,7 @@ sisfb_handle_command(struct sis_video_info *ivideo, struct sisfb_cmd *sisfb_comm
 }
 
 #ifndef MODULE
-SISINITSTATIC int __init
-sisfb_setup(char *options)
+static int __init sisfb_setup(char *options)
 {
 	char *this_opt;
 
@@ -4086,9 +4065,9 @@ sisfb_setup(char *options)
 #endif
 
 static int __devinit
-sisfb_check_rom(SIS_IOTYPE1 *rom_base, struct sis_video_info *ivideo)
+sisfb_check_rom(void __iomem *rom_base, struct sis_video_info *ivideo)
 {
-	SIS_IOTYPE1 *rom;
+	void __iomem *rom;
 	int romptr;
 
 	if((readb(rom_base) != 0x55) || (readb(rom_base + 1) != 0xaa))
@@ -4117,10 +4096,9 @@ static unsigned char * __devinit
 sisfb_find_rom(struct pci_dev *pdev)
 {
 	struct sis_video_info *ivideo = pci_get_drvdata(pdev);
-	SIS_IOTYPE1 *rom_base;
+	void __iomem *rom_base;
 	unsigned char *myrombase = NULL;
 	u32 temp;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,11)
 	size_t romsize;
 
 	/* First, try the official pci ROM functions (except
@@ -4151,7 +4129,6 @@ sisfb_find_rom(struct pci_dev *pdev)
 	}
 
 	if(myrombase) return myrombase;
-#endif
 
 	/* Otherwise do it the conventional way. */
 
@@ -4225,7 +4202,7 @@ sisfb_post_map_vram(struct sis_video_info *ivideo, unsigned int *mapsize,
 static int __devinit
 sisfb_post_300_buswidth(struct sis_video_info *ivideo)
 {
-	SIS_IOTYPE1 *FBAddress = ivideo->video_vbase;
+	void __iomem *FBAddress = ivideo->video_vbase;
 	unsigned short temp;
 	unsigned char reg;
 	int i, j;
@@ -4273,7 +4250,7 @@ sisfb_post_300_rwtest(struct sis_video_info *ivideo, int iteration, int buswidth
 			int PseudoRankCapacity, int PseudoAdrPinCount,
 			unsigned int mapsize)
 {
-	SIS_IOTYPE1 *FBAddr = ivideo->video_vbase;
+	void __iomem *FBAddr = ivideo->video_vbase;
 	unsigned short sr14;
 	unsigned int k, RankCapacity, PageCapacity, BankNumHigh, BankNumMid;
 	unsigned int PhysicalAdrOtherPage, PhysicalAdrHigh, PhysicalAdrHalfPage;
@@ -5829,7 +5806,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	ivideo->engineok = 0;
 
 	ivideo->sisfb_was_boot_device = 0;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12))
+
 	if(pdev->resource[PCI_ROM_RESOURCE].flags & IORESOURCE_ROM_SHADOW) {
 		if(ivideo->sisvga_enabled)
 			ivideo->sisfb_was_boot_device = 1;
@@ -5840,7 +5817,6 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 				"as the primary VGA device\n");
 		}
 	}
-#endif
 
 	ivideo->sisfb_parm_mem = sisfb_parm_mem;
 	ivideo->sisfb_accel = sisfb_accel;
@@ -6010,7 +5986,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		ivideo->modeprechange = reg & 0x7f;
 	} else if(ivideo->sisvga_enabled) {
 #if defined(__i386__) || defined(__x86_64__)
-		unsigned char SIS_IOTYPE2 *tt = ioremap(0x400, 0x100);
+		unsigned char __iomem *tt = ioremap(0x400, 0x100);
 		if(tt) {
 			ivideo->modeprechange = readb(tt + 0x49);
 			iounmap(tt);
@@ -6503,7 +6479,7 @@ static struct pci_driver sisfb_driver = {
 	.remove 	= __devexit_p(sisfb_remove)
 };
 
-SISINITSTATIC int __init sisfb_init(void)
+static int __init sisfb_init(void)
 {
 #ifndef MODULE
 	char *options = NULL;
diff --git a/drivers/video/sis/sis_main.h b/drivers/video/sis/sis_main.h
index 3e3b7fa05d6..9540e977270 100644
--- a/drivers/video/sis/sis_main.h
+++ b/drivers/video/sis/sis_main.h
@@ -665,11 +665,11 @@ static struct _customttable {
 
 /* Interface used by the world */
 #ifndef MODULE
-SISINITSTATIC int sisfb_setup(char *options);
+static int sisfb_setup(char *options);
 #endif
 
 /* Interface to the low level console driver */
-SISINITSTATIC int sisfb_init(void);
+static int sisfb_init(void);
 
 /* fbdev routines */
 static int	sisfb_get_fix(struct fb_fix_screeninfo *fix, int con,
diff --git a/drivers/video/sis/vgatypes.h b/drivers/video/sis/vgatypes.h
index b532fbd2b04..81a22eaabfd 100644
--- a/drivers/video/sis/vgatypes.h
+++ b/drivers/video/sis/vgatypes.h
@@ -53,10 +53,6 @@
 #ifndef _VGATYPES_H_
 #define _VGATYPES_H_
 
-#ifdef SIS_LINUX_KERNEL
-#include <linux/version.h>
-#endif
-
 #define SISIOMEMTYPE
 
 #ifdef SIS_LINUX_KERNEL
diff --git a/drivers/video/skeletonfb.c b/drivers/video/skeletonfb.c
index 62321458f71..df5336561d1 100644
--- a/drivers/video/skeletonfb.c
+++ b/drivers/video/skeletonfb.c
@@ -675,13 +675,13 @@ static struct fb_ops xxxfb_ops = {
      *  Initialization
      */
 
-/* static int __init xxfb_probe (struct device *device) -- for platform devs */
+/* static int __init xxfb_probe (struct platform_device *pdev) -- for platform devs */
 static int __devinit xxxfb_probe(struct pci_dev *dev,
 			      const struct pci_device_id *ent)
 {
     struct fb_info *info;
     struct xxx_par *par;
-    struct device* device = &dev->dev; /* for pci drivers */
+    struct device *device = &dev->dev; /* or &pdev->dev */
     int cmap_len, retval;	
    
     /*
@@ -824,18 +824,18 @@ static int __devinit xxxfb_probe(struct pci_dev *dev,
 	return -EINVAL;
     printk(KERN_INFO "fb%d: %s frame buffer device\n", info->node,
 	   info->fix.id);
-    pci_set_drvdata(dev, info); /* or dev_set_drvdata(device, info) */
+    pci_set_drvdata(dev, info); /* or platform_set_drvdata(pdev, info) */
     return 0;
 }
 
     /*
      *  Cleanup
      */
-/* static void __devexit xxxfb_remove(struct device *device) */
+/* static void __devexit xxxfb_remove(struct platform_device *pdev) */
 static void __devexit xxxfb_remove(struct pci_dev *dev)
 {
 	struct fb_info *info = pci_get_drvdata(dev);
-	/* or dev_get_drvdata(device); */
+	/* or platform_get_drvdata(pdev); */
 
 	if (info) {
 		unregister_framebuffer(info);
@@ -961,18 +961,17 @@ static int xxxfb_resume(struct platform_dev *dev)
 #define xxxfb_resume NULL
 #endif /* CONFIG_PM */
 
-static struct device_driver xxxfb_driver = {
-	.name = "xxxfb",
-	.bus  = &platform_bus_type,
+static struct platform_device_driver xxxfb_driver = {
 	.probe = xxxfb_probe,
 	.remove = xxxfb_remove,
 	.suspend = xxxfb_suspend, /* optional but recommended */
 	.resume = xxxfb_resume,   /* optional but recommended */
+	.driver = {
+		.name = "xxxfb",
+	},
 };
 
-static struct platform_device xxxfb_device = {
-	.name = "xxxfb",
-};
+static struct platform_device *xxxfb_device;
 
 #ifndef MODULE
     /*
@@ -1002,12 +1001,16 @@ static int __init xxxfb_init(void)
 		return -ENODEV;
 	xxxfb_setup(option);
 #endif
-	ret = driver_register(&xxxfb_driver);
+	ret = platform_driver_register(&xxxfb_driver);
 
 	if (!ret) {
-		ret = platform_device_register(&xxxfb_device);
-		if (ret)
-			driver_unregister(&xxxfb_driver);
+		xxxfb_device = platform_device_register_simple("xxxfb", 0,
+								NULL, 0);
+
+		if (IS_ERR(xxxfb_device)) {
+			platform_driver_unregister(&xxxfb_driver);
+			ret = PTR_ERR(xxxfb_device);
+		}
 	}
 
 	return ret;
@@ -1015,8 +1018,8 @@ static int __init xxxfb_init(void)
 
 static void __exit xxxfb_exit(void)
 {
-	platform_device_unregister(&xxxfb_device);
-	driver_unregister(&xxxfb_driver);
+	platform_device_unregister(xxxfb_device);
+	platform_driver_unregister(&xxxfb_driver);
 }
 #endif /* CONFIG_PCI */
 
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index 15d4a768b1f..f94ae84a58c 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -48,10 +48,15 @@ enum sm501_controller {
 	HEAD_PANEL	= 1,
 };
 
-/* SM501 memory address */
+/* SM501 memory address.
+ *
+ * This structure is used to track memory usage within the SM501 framebuffer
+ * allocation. The sm_addr field is stored as an offset as it is often used
+ * against both the physical and mapped addresses.
+ */
 struct sm501_mem {
 	unsigned long	 size;
-	unsigned long	 sm_addr;
+	unsigned long	 sm_addr;	/* offset from base of sm501 fb. */
 	void __iomem	*k_addr;
 };
 
@@ -142,31 +147,68 @@ static inline void sm501fb_sync_regs(struct sm501fb_info *info)
 static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 			   unsigned int why, size_t size)
 {
-	unsigned int ptr = 0;
+	struct sm501fb_par *par;
+	struct fb_info *fbi;
+	unsigned int ptr;
+	unsigned int end;
 
 	switch (why) {
 	case SM501_MEMF_CURSOR:
 		ptr = inf->fbmem_len - size;
-		inf->fbmem_len = ptr;
+		inf->fbmem_len = ptr;	/* adjust available memory. */
 		break;
 
 	case SM501_MEMF_PANEL:
 		ptr = inf->fbmem_len - size;
-		if (ptr < inf->fb[0]->fix.smem_len)
+		fbi = inf->fb[HEAD_CRT];
+
+		/* round down, some programs such as directfb do not draw
+		 * 0,0 correctly unless the start is aligned to a page start.
+		 */
+
+		if (ptr > 0)
+			ptr &= ~(PAGE_SIZE - 1);
+
+		if (fbi && ptr < fbi->fix.smem_len)
+			return -ENOMEM;
+
+		if (ptr < 0)
 			return -ENOMEM;
 
 		break;
 
 	case SM501_MEMF_CRT:
 		ptr = 0;
+
+		/* check to see if we have panel memory allocated
+		 * which would put an limit on available memory. */
+
+		fbi = inf->fb[HEAD_PANEL];
+		if (fbi) {
+			par = fbi->par;
+			end = par->screen.k_addr ? par->screen.sm_addr : inf->fbmem_len;
+		} else
+			end = inf->fbmem_len;
+
+		if ((ptr + size) > end)
+			return -ENOMEM;
+
 		break;
 
 	case SM501_MEMF_ACCEL:
-		ptr = inf->fb[0]->fix.smem_len;
+		fbi = inf->fb[HEAD_CRT];
+		ptr = fbi ? fbi->fix.smem_len : 0;
+
+		fbi = inf->fb[HEAD_PANEL];
+		if (fbi) {
+			par = fbi->par;
+			end = par->screen.sm_addr;
+		} else
+			end = inf->fbmem_len;
 
-		if ((ptr + size) >
-		    (inf->fb[1]->fix.smem_start - inf->fbmem_res->start))
+		if ((ptr + size) > end)
 			return -ENOMEM;
+
 		break;
 
 	default:
@@ -663,15 +705,25 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 		sm501fb_sync_regs(fbi);
 		mdelay(10);
 
+		/* VBIASEN */
+
 		if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
-			control |= SM501_DC_PANEL_CONTROL_BIAS;	/* VBIASEN */
+			if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
+				control &= ~SM501_DC_PANEL_CONTROL_BIAS;
+			else
+				control |= SM501_DC_PANEL_CONTROL_BIAS;
+
 			writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
 
 		if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
-			control |= SM501_DC_PANEL_CONTROL_FPEN;
+			if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
+				control &= ~SM501_DC_PANEL_CONTROL_FPEN;
+			else
+				control |= SM501_DC_PANEL_CONTROL_FPEN;
+
 			writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
@@ -679,14 +731,22 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 	} else if (!to && (control & SM501_DC_PANEL_CONTROL_VDD) != 0) {
 		/* disable panel power */
 		if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
-			control &= ~SM501_DC_PANEL_CONTROL_FPEN;
+			if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
+				control |= SM501_DC_PANEL_CONTROL_FPEN;
+			else
+				control &= ~SM501_DC_PANEL_CONTROL_FPEN;
+
 			writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
 
 		if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
-			control &= ~SM501_DC_PANEL_CONTROL_BIAS;
+			if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
+				control |= SM501_DC_PANEL_CONTROL_BIAS;
+			else
+				control &= ~SM501_DC_PANEL_CONTROL_BIAS;
+
 			writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
@@ -1210,39 +1270,6 @@ static struct fb_ops sm501fb_ops_pnl = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
-/* sm501fb_info_alloc
- *
- * creates and initialises an sm501fb_info structure
-*/
-
-static struct sm501fb_info *sm501fb_info_alloc(struct fb_info *fbinfo_crt,
-					       struct fb_info *fbinfo_pnl)
-{
-	struct sm501fb_info *info;
-	struct sm501fb_par  *par;
-
-	info = kzalloc(sizeof(struct sm501fb_info), GFP_KERNEL);
-	if (info) {
-		/* set the references back */
-
-		par = fbinfo_crt->par;
-		par->info = info;
-		par->head = HEAD_CRT;
-		fbinfo_crt->pseudo_palette = &par->pseudo_palette;
-
-		par = fbinfo_pnl->par;
-		par->info = info;
-		par->head = HEAD_PANEL;
-		fbinfo_pnl->pseudo_palette = &par->pseudo_palette;
-
-		/* store the two fbs into our info */
-		info->fb[HEAD_CRT] = fbinfo_crt;
-		info->fb[HEAD_PANEL] = fbinfo_pnl;
-	}
-
-	return info;
-}
-
 /* sm501_init_cursor
  *
  * initialise hw cursor parameters
@@ -1250,10 +1277,16 @@ static struct sm501fb_info *sm501fb_info_alloc(struct fb_info *fbinfo_crt,
 
 static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
 {
-	struct sm501fb_par *par = fbi->par;
-	struct sm501fb_info *info = par->info;
+	struct sm501fb_par *par;
+	struct sm501fb_info *info;
 	int ret;
 
+	if (fbi == NULL)
+		return 0;
+
+	par = fbi->par;
+	info = par->info;
+
 	par->cursor_regs = info->regs + reg_base;
 
 	ret = sm501_alloc_mem(info, &par->cursor, SM501_MEMF_CURSOR, 1024);
@@ -1281,13 +1314,10 @@ static int sm501fb_start(struct sm501fb_info *info,
 			 struct platform_device *pdev)
 {
 	struct resource	*res;
-	struct device *dev;
+	struct device *dev = &pdev->dev;
 	int k;
 	int ret;
 
-	info->dev = dev = &pdev->dev;
-	platform_set_drvdata(pdev, info);
-
 	info->irq = ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
 		/* we currently do not use the IRQ */
@@ -1390,11 +1420,6 @@ static void sm501fb_stop(struct sm501fb_info *info)
 	kfree(info->regs_res);
 }
 
-static void sm501fb_info_release(struct sm501fb_info *info)
-{
-	kfree(info);
-}
-
 static int sm501fb_init_fb(struct fb_info *fb,
 			   enum sm501_controller head,
 			   const char *fbname)
@@ -1539,36 +1564,93 @@ static struct sm501_platdata_fb sm501fb_def_pdata = {
 static char driver_name_crt[] = "sm501fb-crt";
 static char driver_name_pnl[] = "sm501fb-panel";
 
-static int __init sm501fb_probe(struct platform_device *pdev)
+static int __devinit sm501fb_probe_one(struct sm501fb_info *info,
+				       enum sm501_controller head)
 {
-	struct sm501fb_info *info;
-	struct device	    *dev = &pdev->dev;
-	struct fb_info	    *fbinfo_crt;
-	struct fb_info	    *fbinfo_pnl;
-	int		     ret;
+	unsigned char *name = (head == HEAD_CRT) ? "crt" : "panel";
+	struct sm501_platdata_fbsub *pd;
+	struct sm501fb_par *par;
+	struct fb_info *fbi;
 
-	/* allocate our framebuffers */
+	pd = (head == HEAD_CRT) ? info->pdata->fb_crt : info->pdata->fb_pnl;
+
+	/* Do not initialise if we've not been given any platform data */
+	if (pd == NULL) {
+		dev_info(info->dev, "no data for fb %s (disabled)\n", name);
+		return 0;
+	}
 
-	fbinfo_crt = framebuffer_alloc(sizeof(struct sm501fb_par), dev);
-	if (fbinfo_crt == NULL) {
-		dev_err(dev, "cannot allocate crt framebuffer\n");
+	fbi = framebuffer_alloc(sizeof(struct sm501fb_par), info->dev);
+	if (fbi == NULL) {
+		dev_err(info->dev, "cannot allocate %s framebuffer\n", name);
 		return -ENOMEM;
 	}
 
-	fbinfo_pnl = framebuffer_alloc(sizeof(struct sm501fb_par), dev);
-	if (fbinfo_pnl == NULL) {
-		dev_err(dev, "cannot allocate panel framebuffer\n");
-		ret = -ENOMEM;
-		goto fbinfo_crt_alloc_fail;
+	par = fbi->par;
+	par->info = info;
+	par->head = head;
+	fbi->pseudo_palette = &par->pseudo_palette;
+
+	info->fb[head] = fbi;
+
+	return 0;
+}
+
+/* Free up anything allocated by sm501fb_init_fb */
+
+static void sm501_free_init_fb(struct sm501fb_info *info,
+				enum sm501_controller head)
+{
+	struct fb_info *fbi = info->fb[head];
+
+	fb_dealloc_cmap(&fbi->cmap);
+}
+
+static int __devinit sm501fb_start_one(struct sm501fb_info *info,
+				       enum sm501_controller head,
+				       const char *drvname)
+{
+	struct fb_info *fbi = info->fb[head];
+	int ret;
+
+	if (!fbi)
+		return 0;
+
+	ret = sm501fb_init_fb(info->fb[head], head, drvname);
+	if (ret) {
+		dev_err(info->dev, "cannot initialise fb %s\n", drvname);
+		return ret;
+	}
+
+	ret = register_framebuffer(info->fb[head]);
+	if (ret) {
+		dev_err(info->dev, "failed to register fb %s\n", drvname);
+		sm501_free_init_fb(info, head);
+		return ret;
 	}
 
-	info = sm501fb_info_alloc(fbinfo_crt, fbinfo_pnl);
-	if (info == NULL) {
-		dev_err(dev, "cannot allocate par\n");
-		ret = -ENOMEM;
-		goto sm501fb_alloc_fail;
+	dev_info(info->dev, "fb%d: %s frame buffer\n", fbi->node, fbi->fix.id);
+
+	return 0;
+}
+
+static int __devinit sm501fb_probe(struct platform_device *pdev)
+{
+	struct sm501fb_info *info;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	/* allocate our framebuffers */
+
+	info = kzalloc(sizeof(struct sm501fb_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev, "failed to allocate state\n");
+		return -ENOMEM;
 	}
 
+	info->dev = dev = &pdev->dev;
+	platform_set_drvdata(pdev, info);
+
 	if (dev->parent->platform_data) {
 		struct sm501_platdata *pd = dev->parent->platform_data;
 		info->pdata = pd->fb;
@@ -1579,90 +1661,88 @@ static int __init sm501fb_probe(struct platform_device *pdev)
 		info->pdata = &sm501fb_def_pdata;
 	}
 
-	/* start the framebuffers */
+	/* probe for the presence of each panel */
 
-	ret = sm501fb_start(info, pdev);
-	if (ret) {
-		dev_err(dev, "cannot initialise SM501\n");
-		goto sm501fb_start_fail;
+	ret = sm501fb_probe_one(info, HEAD_CRT);
+	if (ret < 0) {
+		dev_err(dev, "failed to probe CRT\n");
+		goto err_alloc;
 	}
 
-	/* CRT framebuffer setup */
+	ret = sm501fb_probe_one(info, HEAD_PANEL);
+	if (ret < 0) {
+		dev_err(dev, "failed to probe PANEL\n");
+		goto err_probed_crt;
+	}
 
-	ret = sm501fb_init_fb(fbinfo_crt, HEAD_CRT, driver_name_crt);
-	if (ret) {
-		dev_err(dev, "cannot initialise CRT fb\n");
-		goto sm501fb_start_fail;
+	if (info->fb[HEAD_PANEL] == NULL &&
+	    info->fb[HEAD_CRT] == NULL) {
+		dev_err(dev, "no framebuffers found\n");
+		goto err_alloc;
 	}
 
-	/* Panel framebuffer setup */
+	/* get the resources for both of the framebuffers */
 
-	ret = sm501fb_init_fb(fbinfo_pnl, HEAD_PANEL, driver_name_pnl);
+	ret = sm501fb_start(info, pdev);
 	if (ret) {
-		dev_err(dev, "cannot initialise Panel fb\n");
-		goto sm501fb_start_fail;
+		dev_err(dev, "cannot initialise SM501\n");
+		goto err_probed_panel;
 	}
 
-	/* register framebuffers */
-
-	ret = register_framebuffer(fbinfo_crt);
-	if (ret < 0) {
-		dev_err(dev, "failed to register CRT fb (%d)\n", ret);
-		goto register_crt_fail;
+	ret = sm501fb_start_one(info, HEAD_CRT, driver_name_crt);
+	if (ret) {
+		dev_err(dev, "failed to start CRT\n");
+		goto err_started;
 	}
 
-	ret = register_framebuffer(fbinfo_pnl);
-	if (ret < 0) {
-		dev_err(dev, "failed to register panel fb (%d)\n", ret);
-		goto register_pnl_fail;
+	ret = sm501fb_start_one(info, HEAD_PANEL, driver_name_pnl);
+	if (ret) {
+		dev_err(dev, "failed to start Panel\n");
+		goto err_started_crt;
 	}
 
-	dev_info(dev, "fb%d: %s frame buffer device\n",
-		 fbinfo_crt->node, fbinfo_crt->fix.id);
-
-	dev_info(dev, "fb%d: %s frame buffer device\n",
-	       fbinfo_pnl->node, fbinfo_pnl->fix.id);
-
 	/* create device files */
 
 	ret = device_create_file(dev, &dev_attr_crt_src);
 	if (ret)
-		goto crtsrc_fail;
+		goto err_started_panel;
 
 	ret = device_create_file(dev, &dev_attr_fbregs_pnl);
 	if (ret)
-		goto fbregs_pnl_fail;
+		goto err_attached_crtsrc_file;
 
 	ret = device_create_file(dev, &dev_attr_fbregs_crt);
 	if (ret)
-		goto fbregs_crt_fail;
+		goto err_attached_pnlregs_file;
 
 	/* we registered, return ok */
 	return 0;
 
- fbregs_crt_fail:
+err_attached_pnlregs_file:
 	device_remove_file(dev, &dev_attr_fbregs_pnl);
 
- fbregs_pnl_fail:
+err_attached_crtsrc_file:
 	device_remove_file(dev, &dev_attr_crt_src);
 
- crtsrc_fail:
-	unregister_framebuffer(fbinfo_pnl);
+err_started_panel:
+	unregister_framebuffer(info->fb[HEAD_PANEL]);
+	sm501_free_init_fb(info, HEAD_PANEL);
 
- register_pnl_fail:
-	unregister_framebuffer(fbinfo_crt);
+err_started_crt:
+	unregister_framebuffer(info->fb[HEAD_CRT]);
+	sm501_free_init_fb(info, HEAD_CRT);
 
- register_crt_fail:
+err_started:
 	sm501fb_stop(info);
 
- sm501fb_start_fail:
-	sm501fb_info_release(info);
+err_probed_panel:
+	framebuffer_release(info->fb[HEAD_PANEL]);
 
- sm501fb_alloc_fail:
-	framebuffer_release(fbinfo_pnl);
+err_probed_crt:
+	framebuffer_release(info->fb[HEAD_CRT]);
 
- fbinfo_crt_alloc_fail:
-	framebuffer_release(fbinfo_crt);
+err_alloc:
+	kfree(info);
 
 	return ret;
 }
@@ -1681,11 +1761,14 @@ static int sm501fb_remove(struct platform_device *pdev)
 	device_remove_file(&pdev->dev, &dev_attr_fbregs_pnl);
 	device_remove_file(&pdev->dev, &dev_attr_crt_src);
 
+	sm501_free_init_fb(info, HEAD_CRT);
+	sm501_free_init_fb(info, HEAD_PANEL);
+
 	unregister_framebuffer(fbinfo_crt);
 	unregister_framebuffer(fbinfo_pnl);
 
 	sm501fb_stop(info);
-	sm501fb_info_release(info);
+	kfree(info);
 
 	framebuffer_release(fbinfo_pnl);
 	framebuffer_release(fbinfo_crt);
diff --git a/drivers/video/sticore.h b/drivers/video/sticore.h
index 1a9a60c74be..7fe5be4bc70 100644
--- a/drivers/video/sticore.h
+++ b/drivers/video/sticore.h
@@ -352,8 +352,6 @@ struct sti_struct *sti_get_rom(unsigned int index); /* 0: default sti */
 
 /* functions to call the STI ROM directly */
 
-int  sti_init_graph(struct sti_struct *sti);
-void sti_inq_conf(struct sti_struct *sti);
 void sti_putc(struct sti_struct *sti, int c, int y, int x);
 void sti_set(struct sti_struct *sti, int src_y, int src_x,
 	     int height, int width, u8 color);
diff --git a/drivers/video/stifb.c b/drivers/video/stifb.c
index 598d35eff93..16648140241 100644
--- a/drivers/video/stifb.c
+++ b/drivers/video/stifb.c
@@ -1078,8 +1078,7 @@ static struct fb_ops stifb_ops = {
  *  Initialization
  */
 
-int __init
-stifb_init_fb(struct sti_struct *sti, int bpp_pref)
+static int __init stifb_init_fb(struct sti_struct *sti, int bpp_pref)
 {
 	struct fb_fix_screeninfo *fix;
 	struct fb_var_screeninfo *var;
@@ -1315,8 +1314,7 @@ static int stifb_disabled __initdata;
 int __init
 stifb_setup(char *options);
 
-int __init
-stifb_init(void)
+static int __init stifb_init(void)
 {
 	struct sti_struct *sti;
 	struct sti_struct *def_sti;
diff --git a/drivers/video/tdfxfb.c b/drivers/video/tdfxfb.c
index ea9f19d2559..77aafcfae03 100644
--- a/drivers/video/tdfxfb.c
+++ b/drivers/video/tdfxfb.c
@@ -836,16 +836,12 @@ static int tdfxfb_pan_display(struct fb_var_screeninfo *var,
 	struct tdfx_par *par = info->par;
 	u32 addr = var->yoffset * info->fix.line_length;
 
-	if (nopan || var->xoffset || (var->yoffset > var->yres_virtual))
-		return -EINVAL;
-	if ((var->yoffset + var->yres > var->yres_virtual && nowrap))
+	if (nopan || var->xoffset)
 		return -EINVAL;
 
 	banshee_make_room(par, 1);
 	tdfx_outl(par, VIDDESKSTART, addr);
 
-	info->var.xoffset = var->xoffset;
-	info->var.yoffset = var->yoffset;
 	return 0;
 }
 
@@ -1426,6 +1422,8 @@ MODULE_LICENSE("GPL");
 module_param(hwcursor, int, 0644);
 MODULE_PARM_DESC(hwcursor, "Enable hardware cursor "
 			"(1=enable, 0=disable, default=1)");
+module_param(mode_option, charp, 0);
+MODULE_PARM_DESC(mode_option, "Initial video mode e.g. '648x480-8@60'");
 #ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "Disable MTRR support (default: enabled)");
diff --git a/drivers/video/tridentfb.c b/drivers/video/tridentfb.c
index beefab2992c..479b2e79ad6 100644
--- a/drivers/video/tridentfb.c
+++ b/drivers/video/tridentfb.c
@@ -1,5 +1,5 @@
 /*
- * Frame buffer driver for Trident Blade and Image series
+ * Frame buffer driver for Trident TGUI, Blade and Image series
  *
  * Copyright 2001, 2002 - Jani Monoses   <jani@iv.ro>
  *
@@ -13,7 +13,6 @@
  *	code, suggestions
  * TODO:
  *	timing value tweaking so it looks good on every monitor in every mode
- *	TGUI acceleration
  */
 
 #include <linux/module.h>
@@ -22,25 +21,26 @@
 #include <linux/pci.h>
 
 #include <linux/delay.h>
+#include <video/vga.h>
 #include <video/trident.h>
 
-#define VERSION		"0.7.8-NEWAPI"
-
 struct tridentfb_par {
 	void __iomem *io_virt;	/* iospace virtual memory address */
+	u32 pseudo_pal[16];
+	int chip_id;
+	int flatpanel;
+	void (*init_accel) (struct tridentfb_par *, int, int);
+	void (*wait_engine) (struct tridentfb_par *);
+	void (*fill_rect)
+		(struct tridentfb_par *par, u32, u32, u32, u32, u32, u32);
+	void (*copy_rect)
+		(struct tridentfb_par *par, u32, u32, u32, u32, u32, u32);
+	void (*image_blit)
+		(struct tridentfb_par *par, const char*,
+		 u32, u32, u32, u32, u32, u32);
+	unsigned char eng_oper;	/* engine operation... */
 };
 
-static unsigned char eng_oper;	/* engine operation... */
-static struct fb_ops tridentfb_ops;
-
-static struct tridentfb_par default_par;
-
-/* FIXME:kmalloc these 3 instead */
-static struct fb_info fb_info;
-static u32 pseudo_pal[16];
-
-static struct fb_var_screeninfo default_var;
-
 static struct fb_fix_screeninfo tridentfb_fix = {
 	.id = "Trident",
 	.type = FB_TYPE_PACKED_PIXELS,
@@ -49,27 +49,22 @@ static struct fb_fix_screeninfo tridentfb_fix = {
 	.accel = FB_ACCEL_NONE,
 };
 
-static int chip_id;
-
-static int defaultaccel;
-static int displaytype;
-
 /* defaults which are normally overriden by user values */
 
 /* video mode */
-static char *mode_option __devinitdata = "640x480";
-static int bpp = 8;
+static char *mode_option __devinitdata = "640x480-8@60";
+static int bpp __devinitdata = 8;
 
-static int noaccel;
+static int noaccel __devinitdata;
 
 static int center;
 static int stretch;
 
-static int fp;
-static int crt;
+static int fp __devinitdata;
+static int crt __devinitdata;
 
-static int memsize;
-static int memdiff;
+static int memsize __devinitdata;
+static int memdiff __devinitdata;
 static int nativex;
 
 module_param(mode_option, charp, 0);
@@ -84,25 +79,53 @@ module_param(memsize, int, 0);
 module_param(memdiff, int, 0);
 module_param(nativex, int, 0);
 module_param(fp, int, 0);
+MODULE_PARM_DESC(fp, "Define if flatpanel is connected");
 module_param(crt, int, 0);
+MODULE_PARM_DESC(crt, "Define if CRT is connected");
+
+static inline int is_oldclock(int id)
+{
+	return	(id == TGUI9440) ||
+		(id == TGUI9660) ||
+		(id == CYBER9320);
+}
+
+static inline int is_oldprotect(int id)
+{
+	return	is_oldclock(id) ||
+		(id == PROVIDIA9685) ||
+		(id == CYBER9382) ||
+		(id == CYBER9385);
+}
+
+static inline int is_blade(int id)
+{
+	return	(id == BLADE3D) ||
+		(id == CYBERBLADEE4) ||
+		(id == CYBERBLADEi7) ||
+		(id == CYBERBLADEi7D) ||
+		(id == CYBERBLADEi1) ||
+		(id == CYBERBLADEi1D) ||
+		(id == CYBERBLADEAi1) ||
+		(id == CYBERBLADEAi1D);
+}
 
-static int chip3D;
-static int chipcyber;
+static inline int is_xp(int id)
+{
+	return	(id == CYBERBLADEXPAi1) ||
+		(id == CYBERBLADEXPm8) ||
+		(id == CYBERBLADEXPm16);
+}
 
-static int is3Dchip(int id)
+static inline int is3Dchip(int id)
 {
-	return ((id == BLADE3D) || (id == CYBERBLADEE4) ||
-		(id == CYBERBLADEi7) || (id == CYBERBLADEi7D) ||
+	return	is_blade(id) || is_xp(id) ||
 		(id == CYBER9397) || (id == CYBER9397DVD) ||
 		(id == CYBER9520) || (id == CYBER9525DVD) ||
-		(id == IMAGE975) || (id == IMAGE985) ||
-		(id == CYBERBLADEi1) || (id == CYBERBLADEi1D) ||
-		(id == CYBERBLADEAi1) || (id == CYBERBLADEAi1D) ||
-		(id == CYBERBLADEXPm8) || (id == CYBERBLADEXPm16) ||
-		(id == CYBERBLADEXPAi1));
+		(id == IMAGE975) || (id == IMAGE985);
 }
 
-static int iscyber(int id)
+static inline int iscyber(int id)
 {
 	switch (id) {
 	case CYBER9388:
@@ -122,12 +145,7 @@ static int iscyber(int id)
 		return 1;
 
 	case CYBER9320:
-	case TGUI9660:
-	case IMAGE975:
-	case IMAGE985:
-	case BLADE3D:
 	case CYBERBLADEi7:	/* VIA MPV4 integrated version */
-
 	default:
 		/* case CYBERBLDAEXPm8:  Strange */
 		/* case CYBERBLDAEXPm16: Strange */
@@ -135,147 +153,110 @@ static int iscyber(int id)
 	}
 }
 
-#define CRT 0x3D0		/* CRTC registers offset for color display */
-
-#ifndef TRIDENT_MMIO
-	#define TRIDENT_MMIO 1
-#endif
-
-#if TRIDENT_MMIO
-	#define t_outb(val, reg)	writeb(val,((struct tridentfb_par *)(fb_info.par))->io_virt + reg)
-	#define t_inb(reg)	readb(((struct tridentfb_par*)(fb_info.par))->io_virt + reg)
-#else
-	#define t_outb(val, reg) outb(val, reg)
-	#define t_inb(reg) inb(reg)
-#endif
+static inline void t_outb(struct tridentfb_par *p, u8 val, u16 reg)
+{
+	fb_writeb(val, p->io_virt + reg);
+}
 
+static inline u8 t_inb(struct tridentfb_par *p, u16 reg)
+{
+	return fb_readb(p->io_virt + reg);
+}
 
-static struct accel_switch {
-	void (*init_accel) (int, int);
-	void (*wait_engine) (void);
-	void (*fill_rect) (u32, u32, u32, u32, u32, u32);
-	void (*copy_rect) (u32, u32, u32, u32, u32, u32);
-} *acc;
+static inline void writemmr(struct tridentfb_par *par, u16 r, u32 v)
+{
+	fb_writel(v, par->io_virt + r);
+}
 
-#define writemmr(r, v)	writel(v, ((struct tridentfb_par *)fb_info.par)->io_virt + r)
-#define readmmr(r)	readl(((struct tridentfb_par *)fb_info.par)->io_virt + r)
+static inline u32 readmmr(struct tridentfb_par *par, u16 r)
+{
+	return fb_readl(par->io_virt + r);
+}
 
 /*
  * Blade specific acceleration.
  */
 
 #define point(x, y) ((y) << 16 | (x))
-#define STA	0x2120
-#define CMD	0x2144
-#define ROP	0x2148
-#define CLR	0x2160
-#define SR1	0x2100
-#define SR2	0x2104
-#define DR1	0x2108
-#define DR2	0x210C
-
-#define ROP_S	0xCC
-
-static void blade_init_accel(int pitch, int bpp)
+
+static void blade_init_accel(struct tridentfb_par *par, int pitch, int bpp)
 {
 	int v1 = (pitch >> 3) << 20;
-	int tmp = 0, v2;
-	switch (bpp) {
-	case 8:
-		tmp = 0;
-		break;
-	case 15:
-		tmp = 5;
-		break;
-	case 16:
-		tmp = 1;
-		break;
-	case 24:
-	case 32:
-		tmp = 2;
-		break;
-	}
-	v2 = v1 | (tmp << 29);
-	writemmr(0x21C0, v2);
-	writemmr(0x21C4, v2);
-	writemmr(0x21B8, v2);
-	writemmr(0x21BC, v2);
-	writemmr(0x21D0, v1);
-	writemmr(0x21D4, v1);
-	writemmr(0x21C8, v1);
-	writemmr(0x21CC, v1);
-	writemmr(0x216C, 0);
+	int tmp = bpp == 24 ? 2 : (bpp >> 4);
+	int v2 = v1 | (tmp << 29);
+
+	writemmr(par, 0x21C0, v2);
+	writemmr(par, 0x21C4, v2);
+	writemmr(par, 0x21B8, v2);
+	writemmr(par, 0x21BC, v2);
+	writemmr(par, 0x21D0, v1);
+	writemmr(par, 0x21D4, v1);
+	writemmr(par, 0x21C8, v1);
+	writemmr(par, 0x21CC, v1);
+	writemmr(par, 0x216C, 0);
 }
 
-static void blade_wait_engine(void)
+static void blade_wait_engine(struct tridentfb_par *par)
 {
-	while (readmmr(STA) & 0xFA800000) ;
+	while (readmmr(par, STATUS) & 0xFA800000)
+		cpu_relax();
 }
 
-static void blade_fill_rect(u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
+static void blade_fill_rect(struct tridentfb_par *par,
+			    u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
 {
-	writemmr(CLR, c);
-	writemmr(ROP, rop ? 0x66 : ROP_S);
-	writemmr(CMD, 0x20000000 | 1 << 19 | 1 << 4 | 2 << 2);
+	writemmr(par, COLOR, c);
+	writemmr(par, ROP, rop ? ROP_X : ROP_S);
+	writemmr(par, CMD, 0x20000000 | 1 << 19 | 1 << 4 | 2 << 2);
 
-	writemmr(DR1, point(x, y));
-	writemmr(DR2, point(x + w - 1, y + h - 1));
+	writemmr(par, DST1, point(x, y));
+	writemmr(par, DST2, point(x + w - 1, y + h - 1));
 }
 
-static void blade_copy_rect(u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
+static void blade_image_blit(struct tridentfb_par *par, const char *data,
+			     u32 x, u32 y, u32 w, u32 h, u32 c, u32 b)
+{
+	unsigned size = ((w + 31) >> 5) * h;
+
+	writemmr(par, COLOR, c);
+	writemmr(par, BGCOLOR, b);
+	writemmr(par, CMD, 0xa0000000 | 3 << 19);
+
+	writemmr(par, DST1, point(x, y));
+	writemmr(par, DST2, point(x + w - 1, y + h - 1));
+
+	memcpy(par->io_virt + 0x10000, data, 4 * size);
+}
+
+static void blade_copy_rect(struct tridentfb_par *par,
+			    u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
 {
-	u32 s1, s2, d1, d2;
 	int direction = 2;
-	s1 = point(x1, y1);
-	s2 = point(x1 + w - 1, y1 + h - 1);
-	d1 = point(x2, y2);
-	d2 = point(x2 + w - 1, y2 + h - 1);
+	u32 s1 = point(x1, y1);
+	u32 s2 = point(x1 + w - 1, y1 + h - 1);
+	u32 d1 = point(x2, y2);
+	u32 d2 = point(x2 + w - 1, y2 + h - 1);
 
 	if ((y1 > y2) || ((y1 == y2) && (x1 > x2)))
 		direction = 0;
 
-	writemmr(ROP, ROP_S);
-	writemmr(CMD, 0xE0000000 | 1 << 19 | 1 << 4 | 1 << 2 | direction);
+	writemmr(par, ROP, ROP_S);
+	writemmr(par, CMD, 0xE0000000 | 1 << 19 | 1 << 4 | 1 << 2 | direction);
 
-	writemmr(SR1, direction ? s2 : s1);
-	writemmr(SR2, direction ? s1 : s2);
-	writemmr(DR1, direction ? d2 : d1);
-	writemmr(DR2, direction ? d1 : d2);
+	writemmr(par, SRC1, direction ? s2 : s1);
+	writemmr(par, SRC2, direction ? s1 : s2);
+	writemmr(par, DST1, direction ? d2 : d1);
+	writemmr(par, DST2, direction ? d1 : d2);
 }
 
-static struct accel_switch accel_blade = {
-	blade_init_accel,
-	blade_wait_engine,
-	blade_fill_rect,
-	blade_copy_rect,
-};
-
 /*
  * BladeXP specific acceleration functions
  */
 
-#define ROP_P 0xF0
-#define masked_point(x, y) ((y & 0xffff)<<16|(x & 0xffff))
-
-static void xp_init_accel(int pitch, int bpp)
+static void xp_init_accel(struct tridentfb_par *par, int pitch, int bpp)
 {
-	int tmp = 0, v1;
-	unsigned char x = 0;
-
-	switch (bpp) {
-	case 8:
-		x = 0;
-		break;
-	case 16:
-		x = 1;
-		break;
-	case 24:
-		x = 3;
-		break;
-	case 32:
-		x = 2;
-		break;
-	}
+	unsigned char x = bpp == 24 ? 3 : (bpp >> 4);
+	int v1 = pitch << (bpp == 24 ? 20 : (18 + x));
 
 	switch (pitch << (bpp >> 3)) {
 	case 8192:
@@ -293,42 +274,21 @@ static void xp_init_accel(int pitch, int bpp)
 		break;
 	}
 
-	t_outb(x, 0x2125);
-
-	eng_oper = x | 0x40;
-
-	switch (bpp) {
-	case 8:
-		tmp = 18;
-		break;
-	case 15:
-	case 16:
-		tmp = 19;
-		break;
-	case 24:
-	case 32:
-		tmp = 20;
-		break;
-	}
+	t_outb(par, x, 0x2125);
 
-	v1 = pitch << tmp;
+	par->eng_oper = x | 0x40;
 
-	writemmr(0x2154, v1);
-	writemmr(0x2150, v1);
-	t_outb(3, 0x2126);
+	writemmr(par, 0x2154, v1);
+	writemmr(par, 0x2150, v1);
+	t_outb(par, 3, 0x2126);
 }
 
-static void xp_wait_engine(void)
+static void xp_wait_engine(struct tridentfb_par *par)
 {
-	int busy;
-	int count, timeout;
-
-	count = 0;
-	timeout = 0;
-	for (;;) {
-		busy = t_inb(STA) & 0x80;
-		if (busy != 0x80)
-			return;
+	int count = 0;
+	int timeout = 0;
+
+	while (t_inb(par, STATUS) & 0x80) {
 		count++;
 		if (count == 10000000) {
 			/* Timeout */
@@ -336,30 +296,31 @@ static void xp_wait_engine(void)
 			timeout++;
 			if (timeout == 8) {
 				/* Reset engine */
-				t_outb(0x00, 0x2120);
+				t_outb(par, 0x00, STATUS);
 				return;
 			}
 		}
+		cpu_relax();
 	}
 }
 
-static void xp_fill_rect(u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
+static void xp_fill_rect(struct tridentfb_par *par,
+			 u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
 {
-	writemmr(0x2127, ROP_P);
-	writemmr(0x2158, c);
-	writemmr(0x2128, 0x4000);
-	writemmr(0x2140, masked_point(h, w));
-	writemmr(0x2138, masked_point(y, x));
-	t_outb(0x01, 0x2124);
-	t_outb(eng_oper, 0x2125);
+	writemmr(par, 0x2127, ROP_P);
+	writemmr(par, 0x2158, c);
+	writemmr(par, DRAWFL, 0x4000);
+	writemmr(par, OLDDIM, point(h, w));
+	writemmr(par, OLDDST, point(y, x));
+	t_outb(par, 0x01, OLDCMD);
+	t_outb(par, par->eng_oper, 0x2125);
 }
 
-static void xp_copy_rect(u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
+static void xp_copy_rect(struct tridentfb_par *par,
+			 u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
 {
-	int direction;
 	u32 x1_tmp, x2_tmp, y1_tmp, y2_tmp;
-
-	direction = 0x0004;
+	int direction = 0x0004;
 
 	if ((x1 < x2) && (y1 == y2)) {
 		direction |= 0x0200;
@@ -379,103 +340,152 @@ static void xp_copy_rect(u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
 		y2_tmp = y2;
 	}
 
-	writemmr(0x2128, direction);
-	t_outb(ROP_S, 0x2127);
-	writemmr(0x213C, masked_point(y1_tmp, x1_tmp));
-	writemmr(0x2138, masked_point(y2_tmp, x2_tmp));
-	writemmr(0x2140, masked_point(h, w));
-	t_outb(0x01, 0x2124);
+	writemmr(par, DRAWFL, direction);
+	t_outb(par, ROP_S, 0x2127);
+	writemmr(par, OLDSRC, point(y1_tmp, x1_tmp));
+	writemmr(par, OLDDST, point(y2_tmp, x2_tmp));
+	writemmr(par, OLDDIM, point(h, w));
+	t_outb(par, 0x01, OLDCMD);
 }
 
-static struct accel_switch accel_xp = {
-	xp_init_accel,
-	xp_wait_engine,
-	xp_fill_rect,
-	xp_copy_rect,
-};
-
 /*
  * Image specific acceleration functions
  */
-static void image_init_accel(int pitch, int bpp)
+static void image_init_accel(struct tridentfb_par *par, int pitch, int bpp)
 {
-	int tmp = 0;
-	switch (bpp) {
-	case 8:
-		tmp = 0;
-		break;
-	case 15:
-		tmp = 5;
-		break;
-	case 16:
-		tmp = 1;
-		break;
-	case 24:
-	case 32:
-		tmp = 2;
-		break;
-	}
-	writemmr(0x2120, 0xF0000000);
-	writemmr(0x2120, 0x40000000 | tmp);
-	writemmr(0x2120, 0x80000000);
-	writemmr(0x2144, 0x00000000);
-	writemmr(0x2148, 0x00000000);
-	writemmr(0x2150, 0x00000000);
-	writemmr(0x2154, 0x00000000);
-	writemmr(0x2120, 0x60000000 | (pitch << 16) | pitch);
-	writemmr(0x216C, 0x00000000);
-	writemmr(0x2170, 0x00000000);
-	writemmr(0x217C, 0x00000000);
-	writemmr(0x2120, 0x10000000);
-	writemmr(0x2130, (2047 << 16) | 2047);
+	int tmp = bpp == 24 ? 2: (bpp >> 4);
+
+	writemmr(par, 0x2120, 0xF0000000);
+	writemmr(par, 0x2120, 0x40000000 | tmp);
+	writemmr(par, 0x2120, 0x80000000);
+	writemmr(par, 0x2144, 0x00000000);
+	writemmr(par, 0x2148, 0x00000000);
+	writemmr(par, 0x2150, 0x00000000);
+	writemmr(par, 0x2154, 0x00000000);
+	writemmr(par, 0x2120, 0x60000000 | (pitch << 16) | pitch);
+	writemmr(par, 0x216C, 0x00000000);
+	writemmr(par, 0x2170, 0x00000000);
+	writemmr(par, 0x217C, 0x00000000);
+	writemmr(par, 0x2120, 0x10000000);
+	writemmr(par, 0x2130, (2047 << 16) | 2047);
 }
 
-static void image_wait_engine(void)
+static void image_wait_engine(struct tridentfb_par *par)
 {
-	while (readmmr(0x2164) & 0xF0000000) ;
+	while (readmmr(par, 0x2164) & 0xF0000000)
+		cpu_relax();
 }
 
-static void image_fill_rect(u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
+static void image_fill_rect(struct tridentfb_par *par,
+			    u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
 {
-	writemmr(0x2120, 0x80000000);
-	writemmr(0x2120, 0x90000000 | ROP_S);
+	writemmr(par, 0x2120, 0x80000000);
+	writemmr(par, 0x2120, 0x90000000 | ROP_S);
 
-	writemmr(0x2144, c);
+	writemmr(par, 0x2144, c);
 
-	writemmr(DR1, point(x, y));
-	writemmr(DR2, point(x + w - 1, y + h - 1));
+	writemmr(par, DST1, point(x, y));
+	writemmr(par, DST2, point(x + w - 1, y + h - 1));
 
-	writemmr(0x2124, 0x80000000 | 3 << 22 | 1 << 10 | 1 << 9);
+	writemmr(par, 0x2124, 0x80000000 | 3 << 22 | 1 << 10 | 1 << 9);
 }
 
-static void image_copy_rect(u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
+static void image_copy_rect(struct tridentfb_par *par,
+			    u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
 {
-	u32 s1, s2, d1, d2;
-	int direction = 2;
-	s1 = point(x1, y1);
-	s2 = point(x1 + w - 1, y1 + h - 1);
-	d1 = point(x2, y2);
-	d2 = point(x2 + w - 1, y2 + h - 1);
+	int direction = 0x4;
+	u32 s1 = point(x1, y1);
+	u32 s2 = point(x1 + w - 1, y1 + h - 1);
+	u32 d1 = point(x2, y2);
+	u32 d2 = point(x2 + w - 1, y2 + h - 1);
 
 	if ((y1 > y2) || ((y1 == y2) && (x1 > x2)))
 		direction = 0;
 
-	writemmr(0x2120, 0x80000000);
-	writemmr(0x2120, 0x90000000 | ROP_S);
+	writemmr(par, 0x2120, 0x80000000);
+	writemmr(par, 0x2120, 0x90000000 | ROP_S);
 
-	writemmr(SR1, direction ? s2 : s1);
-	writemmr(SR2, direction ? s1 : s2);
-	writemmr(DR1, direction ? d2 : d1);
-	writemmr(DR2, direction ? d1 : d2);
-	writemmr(0x2124, 0x80000000 | 1 << 22 | 1 << 10 | 1 << 7 | direction);
+	writemmr(par, SRC1, direction ? s2 : s1);
+	writemmr(par, SRC2, direction ? s1 : s2);
+	writemmr(par, DST1, direction ? d2 : d1);
+	writemmr(par, DST2, direction ? d1 : d2);
+	writemmr(par, 0x2124,
+		 0x80000000 | 1 << 22 | 1 << 10 | 1 << 7 | direction);
 }
 
-static struct accel_switch accel_image = {
-	image_init_accel,
-	image_wait_engine,
-	image_fill_rect,
-	image_copy_rect,
-};
+/*
+ * TGUI 9440/96XX acceleration
+ */
+
+static void tgui_init_accel(struct tridentfb_par *par, int pitch, int bpp)
+{
+	unsigned char x = bpp == 24 ? 3 : (bpp >> 4);
+
+	/* disable clipping */
+	writemmr(par, 0x2148, 0);
+	writemmr(par, 0x214C, point(4095, 2047));
+
+	switch ((pitch * bpp) / 8) {
+	case 8192:
+	case 512:
+		x |= 0x00;
+		break;
+	case 1024:
+		x |= 0x04;
+		break;
+	case 2048:
+		x |= 0x08;
+		break;
+	case 4096:
+		x |= 0x0C;
+		break;
+	}
+
+	fb_writew(x, par->io_virt + 0x2122);
+}
+
+static void tgui_fill_rect(struct tridentfb_par *par,
+			   u32 x, u32 y, u32 w, u32 h, u32 c, u32 rop)
+{
+	t_outb(par, ROP_P, 0x2127);
+	writemmr(par, OLDCLR, c);
+	writemmr(par, DRAWFL, 0x4020);
+	writemmr(par, OLDDIM, point(w - 1, h - 1));
+	writemmr(par, OLDDST, point(x, y));
+	t_outb(par, 1, OLDCMD);
+}
+
+static void tgui_copy_rect(struct tridentfb_par *par,
+			   u32 x1, u32 y1, u32 x2, u32 y2, u32 w, u32 h)
+{
+	int flags = 0;
+	u16 x1_tmp, x2_tmp, y1_tmp, y2_tmp;
+
+	if ((x1 < x2) && (y1 == y2)) {
+		flags |= 0x0200;
+		x1_tmp = x1 + w - 1;
+		x2_tmp = x2 + w - 1;
+	} else {
+		x1_tmp = x1;
+		x2_tmp = x2;
+	}
+
+	if (y1 < y2) {
+		flags |= 0x0100;
+		y1_tmp = y1 + h - 1;
+		y2_tmp = y2 + h - 1;
+	} else {
+		y1_tmp = y1;
+		y2_tmp = y2;
+	}
+
+	writemmr(par, DRAWFL, 0x4 | flags);
+	t_outb(par, ROP_S, 0x2127);
+	writemmr(par, OLDSRC, point(x1_tmp, y1_tmp));
+	writemmr(par, OLDDST, point(x2_tmp, y2_tmp));
+	writemmr(par, OLDDIM, point(w - 1, h - 1));
+	t_outb(par, 1, OLDCMD);
+}
 
 /*
  * Accel functions called by the upper layers
@@ -484,129 +494,162 @@ static struct accel_switch accel_image = {
 static void tridentfb_fillrect(struct fb_info *info,
 			       const struct fb_fillrect *fr)
 {
-	int bpp = info->var.bits_per_pixel;
-	int col = 0;
+	struct tridentfb_par *par = info->par;
+	int col;
 
-	switch (bpp) {
-	default:
-	case 8:
-		col |= fr->color;
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		cfb_fillrect(info, fr);
+		return;
+	}
+	if (info->var.bits_per_pixel == 8) {
+		col = fr->color;
 		col |= col << 8;
 		col |= col << 16;
-		break;
-	case 16:
+	} else
 		col = ((u32 *)(info->pseudo_palette))[fr->color];
-		break;
-	case 32:
-		col = ((u32 *)(info->pseudo_palette))[fr->color];
-		break;
+
+	par->wait_engine(par);
+	par->fill_rect(par, fr->dx, fr->dy, fr->width,
+		       fr->height, col, fr->rop);
+}
+
+static void tridentfb_imageblit(struct fb_info *info,
+				const struct fb_image *img)
+{
+	struct tridentfb_par *par = info->par;
+	int col, bgcol;
+
+	if ((info->flags & FBINFO_HWACCEL_DISABLED) || img->depth != 1) {
+		cfb_imageblit(info, img);
+		return;
+	}
+	if (info->var.bits_per_pixel == 8) {
+		col = img->fg_color;
+		col |= col << 8;
+		col |= col << 16;
+		bgcol = img->bg_color;
+		bgcol |= bgcol << 8;
+		bgcol |= bgcol << 16;
+	} else {
+		col = ((u32 *)(info->pseudo_palette))[img->fg_color];
+		bgcol = ((u32 *)(info->pseudo_palette))[img->bg_color];
 	}
 
-	acc->fill_rect(fr->dx, fr->dy, fr->width, fr->height, col, fr->rop);
-	acc->wait_engine();
+	par->wait_engine(par);
+	if (par->image_blit)
+		par->image_blit(par, img->data, img->dx, img->dy,
+				img->width, img->height, col, bgcol);
+	else
+		cfb_imageblit(info, img);
 }
+
 static void tridentfb_copyarea(struct fb_info *info,
 			       const struct fb_copyarea *ca)
 {
-	acc->copy_rect(ca->sx, ca->sy, ca->dx, ca->dy, ca->width, ca->height);
-	acc->wait_engine();
+	struct tridentfb_par *par = info->par;
+
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		cfb_copyarea(info, ca);
+		return;
+	}
+	par->wait_engine(par);
+	par->copy_rect(par, ca->sx, ca->sy, ca->dx, ca->dy,
+		       ca->width, ca->height);
+}
+
+static int tridentfb_sync(struct fb_info *info)
+{
+	struct tridentfb_par *par = info->par;
+
+	if (!(info->flags & FBINFO_HWACCEL_DISABLED))
+		par->wait_engine(par);
+	return 0;
 }
-#else /* !CONFIG_FB_TRIDENT_ACCEL */
+#else
 #define tridentfb_fillrect cfb_fillrect
 #define tridentfb_copyarea cfb_copyarea
+#define tridentfb_imageblit cfb_imageblit
 #endif /* CONFIG_FB_TRIDENT_ACCEL */
 
-
 /*
  * Hardware access functions
  */
 
-static inline unsigned char read3X4(int reg)
+static inline unsigned char read3X4(struct tridentfb_par *par, int reg)
 {
-	struct tridentfb_par *par = (struct tridentfb_par *)fb_info.par;
-	writeb(reg, par->io_virt + CRT + 4);
-	return readb(par->io_virt + CRT + 5);
+	return vga_mm_rcrt(par->io_virt, reg);
 }
 
-static inline void write3X4(int reg, unsigned char val)
+static inline void write3X4(struct tridentfb_par *par, int reg,
+			    unsigned char val)
 {
-	struct tridentfb_par *par = (struct tridentfb_par *)fb_info.par;
-	writeb(reg, par->io_virt + CRT + 4);
-	writeb(val, par->io_virt + CRT + 5);
+	vga_mm_wcrt(par->io_virt, reg, val);
 }
 
-static inline unsigned char read3C4(int reg)
+static inline unsigned char read3CE(struct tridentfb_par *par,
+				    unsigned char reg)
 {
-	t_outb(reg, 0x3C4);
-	return t_inb(0x3C5);
+	return vga_mm_rgfx(par->io_virt, reg);
 }
 
-static inline void write3C4(int reg, unsigned char val)
+static inline void writeAttr(struct tridentfb_par *par, int reg,
+			     unsigned char val)
 {
-	t_outb(reg, 0x3C4);
-	t_outb(val, 0x3C5);
+	fb_readb(par->io_virt + VGA_IS1_RC);	/* flip-flop to index */
+	vga_mm_wattr(par->io_virt, reg, val);
 }
 
-static inline unsigned char read3CE(int reg)
+static inline void write3CE(struct tridentfb_par *par, int reg,
+			    unsigned char val)
 {
-	t_outb(reg, 0x3CE);
-	return t_inb(0x3CF);
+	vga_mm_wgfx(par->io_virt, reg, val);
 }
 
-static inline void writeAttr(int reg, unsigned char val)
-{
-	readb(((struct tridentfb_par *)fb_info.par)->io_virt + CRT + 0x0A);	/* flip-flop to index */
-	t_outb(reg, 0x3C0);
-	t_outb(val, 0x3C0);
-}
-
-static inline void write3CE(int reg, unsigned char val)
-{
-	t_outb(reg, 0x3CE);
-	t_outb(val, 0x3CF);
-}
-
-static void enable_mmio(void)
+static void enable_mmio(struct tridentfb_par *par)
 {
 	/* Goto New Mode */
-	outb(0x0B, 0x3C4);
-	inb(0x3C5);
+	vga_io_rseq(0x0B);
 
 	/* Unprotect registers */
-	outb(NewMode1, 0x3C4);
-	outb(0x80, 0x3C5);
+	vga_io_wseq(NewMode1, 0x80);
+	if (!is_oldprotect(par->chip_id))
+		vga_io_wseq(Protection, 0x92);
 
 	/* Enable MMIO */
 	outb(PCIReg, 0x3D4);
 	outb(inb(0x3D5) | 0x01, 0x3D5);
 }
 
-static void disable_mmio(void)
+static void disable_mmio(struct tridentfb_par *par)
 {
 	/* Goto New Mode */
-	t_outb(0x0B, 0x3C4);
-	t_inb(0x3C5);
+	vga_mm_rseq(par->io_virt, 0x0B);
 
 	/* Unprotect registers */
-	t_outb(NewMode1, 0x3C4);
-	t_outb(0x80, 0x3C5);
+	vga_mm_wseq(par->io_virt, NewMode1, 0x80);
+	if (!is_oldprotect(par->chip_id))
+		vga_mm_wseq(par->io_virt, Protection, 0x92);
 
 	/* Disable MMIO */
-	t_outb(PCIReg, 0x3D4);
-	t_outb(t_inb(0x3D5) & ~0x01, 0x3D5);
+	t_outb(par, PCIReg, 0x3D4);
+	t_outb(par, t_inb(par, 0x3D5) & ~0x01, 0x3D5);
 }
 
-#define crtc_unlock()	write3X4(CRTVSyncEnd, read3X4(CRTVSyncEnd) & 0x7F)
+static inline void crtc_unlock(struct tridentfb_par *par)
+{
+	write3X4(par, VGA_CRTC_V_SYNC_END,
+		 read3X4(par, VGA_CRTC_V_SYNC_END) & 0x7F);
+}
 
 /*  Return flat panel's maximum x resolution */
-static int __devinit get_nativex(void)
+static int __devinit get_nativex(struct tridentfb_par *par)
 {
 	int x, y, tmp;
 
 	if (nativex)
 		return nativex;
 
-	tmp = (read3CE(VertStretch) >> 4) & 3;
+	tmp = (read3CE(par, VertStretch) >> 4) & 3;
 
 	switch (tmp) {
 	case 0:
@@ -632,77 +675,92 @@ static int __devinit get_nativex(void)
 }
 
 /* Set pitch */
-static void set_lwidth(int width)
+static inline void set_lwidth(struct tridentfb_par *par, int width)
 {
-	write3X4(Offset, width & 0xFF);
-	write3X4(AddColReg,
-		 (read3X4(AddColReg) & 0xCF) | ((width & 0x300) >> 4));
+	write3X4(par, VGA_CRTC_OFFSET, width & 0xFF);
+	write3X4(par, AddColReg,
+		 (read3X4(par, AddColReg) & 0xCF) | ((width & 0x300) >> 4));
 }
 
 /* For resolutions smaller than FP resolution stretch */
-static void screen_stretch(void)
+static void screen_stretch(struct tridentfb_par *par)
 {
-	if (chip_id != CYBERBLADEXPAi1)
-		write3CE(BiosReg, 0);
+	if (par->chip_id != CYBERBLADEXPAi1)
+		write3CE(par, BiosReg, 0);
 	else
-		write3CE(BiosReg, 8);
-	write3CE(VertStretch, (read3CE(VertStretch) & 0x7C) | 1);
-	write3CE(HorStretch, (read3CE(HorStretch) & 0x7C) | 1);
+		write3CE(par, BiosReg, 8);
+	write3CE(par, VertStretch, (read3CE(par, VertStretch) & 0x7C) | 1);
+	write3CE(par, HorStretch, (read3CE(par, HorStretch) & 0x7C) | 1);
 }
 
 /* For resolutions smaller than FP resolution center */
-static void screen_center(void)
+static inline void screen_center(struct tridentfb_par *par)
 {
-	write3CE(VertStretch, (read3CE(VertStretch) & 0x7C) | 0x80);
-	write3CE(HorStretch, (read3CE(HorStretch) & 0x7C) | 0x80);
+	write3CE(par, VertStretch, (read3CE(par, VertStretch) & 0x7C) | 0x80);
+	write3CE(par, HorStretch, (read3CE(par, HorStretch) & 0x7C) | 0x80);
 }
 
 /* Address of first shown pixel in display memory */
-static void set_screen_start(int base)
+static void set_screen_start(struct tridentfb_par *par, int base)
 {
-	write3X4(StartAddrLow, base & 0xFF);
-	write3X4(StartAddrHigh, (base & 0xFF00) >> 8);
-	write3X4(CRTCModuleTest,
-		 (read3X4(CRTCModuleTest) & 0xDF) | ((base & 0x10000) >> 11));
-	write3X4(CRTHiOrd,
-		 (read3X4(CRTHiOrd) & 0xF8) | ((base & 0xE0000) >> 17));
+	u8 tmp;
+	write3X4(par, VGA_CRTC_START_LO, base & 0xFF);
+	write3X4(par, VGA_CRTC_START_HI, (base & 0xFF00) >> 8);
+	tmp = read3X4(par, CRTCModuleTest) & 0xDF;
+	write3X4(par, CRTCModuleTest, tmp | ((base & 0x10000) >> 11));
+	tmp = read3X4(par, CRTHiOrd) & 0xF8;
+	write3X4(par, CRTHiOrd, tmp | ((base & 0xE0000) >> 17));
 }
 
 /* Set dotclock frequency */
-static void set_vclk(unsigned long freq)
+static void set_vclk(struct tridentfb_par *par, unsigned long freq)
 {
 	int m, n, k;
-	unsigned long f, fi, d, di;
-	unsigned char lo = 0, hi = 0;
+	unsigned long fi, d, di;
+	unsigned char best_m = 0, best_n = 0, best_k = 0;
+	unsigned char hi, lo;
+	unsigned char shift = !is_oldclock(par->chip_id) ? 2 : 1;
 
 	d = 20000;
-	for (k = 2; k >= 0; k--)
-		for (m = 0; m < 63; m++)
-			for (n = 0; n < 128; n++) {
+	for (k = shift; k >= 0; k--)
+		for (m = 1; m < 32; m++) {
+			n = ((m + 2) << shift) - 8;
+			for (n = (n < 0 ? 0 : n); n < 122; n++) {
 				fi = ((14318l * (n + 8)) / (m + 2)) >> k;
-				if ((di = abs(fi - freq)) < d) {
+				di = abs(fi - freq);
+				if (di < d || (di == d && k == best_k)) {
 					d = di;
-					f = fi;
-					lo = n;
-					hi = (k << 6) | m;
+					best_n = n;
+					best_m = m;
+					best_k = k;
 				}
 				if (fi > freq)
 					break;
 			}
-	if (chip3D) {
-		write3C4(ClockHigh, hi);
-		write3C4(ClockLow, lo);
+		}
+
+	if (is_oldclock(par->chip_id)) {
+		lo = best_n | (best_m << 7);
+		hi = (best_m >> 1) | (best_k << 4);
 	} else {
-		outb(lo, 0x43C8);
-		outb(hi, 0x43C9);
+		lo = best_n;
+		hi = best_m | (best_k << 6);
+	}
+
+	if (is3Dchip(par->chip_id)) {
+		vga_mm_wseq(par->io_virt, ClockHigh, hi);
+		vga_mm_wseq(par->io_virt, ClockLow, lo);
+	} else {
+		t_outb(par, lo, 0x43C8);
+		t_outb(par, hi, 0x43C9);
 	}
 	debug("VCLK = %X %X\n", hi, lo);
 }
 
 /* Set number of lines for flat panels*/
-static void set_number_of_lines(int lines)
+static void set_number_of_lines(struct tridentfb_par *par, int lines)
 {
-	int tmp = read3CE(CyberEnhance) & 0x8F;
+	int tmp = read3CE(par, CyberEnhance) & 0x8F;
 	if (lines > 1024)
 		tmp |= 0x50;
 	else if (lines > 768)
@@ -711,24 +769,24 @@ static void set_number_of_lines(int lines)
 		tmp |= 0x20;
 	else if (lines > 480)
 		tmp |= 0x10;
-	write3CE(CyberEnhance, tmp);
+	write3CE(par, CyberEnhance, tmp);
 }
 
 /*
  * If we see that FP is active we assume we have one.
- * Otherwise we have a CRT display.User can override.
+ * Otherwise we have a CRT display. User can override.
  */
-static unsigned int __devinit get_displaytype(void)
+static int __devinit is_flatpanel(struct tridentfb_par *par)
 {
 	if (fp)
-		return DISPLAY_FP;
-	if (crt || !chipcyber)
-		return DISPLAY_CRT;
-	return (read3CE(FPConfig) & 0x10) ? DISPLAY_FP : DISPLAY_CRT;
+		return 1;
+	if (crt || !iscyber(par->chip_id))
+		return 0;
+	return (read3CE(par, FPConfig) & 0x10) ? 1 : 0;
 }
 
 /* Try detecting the video memory size */
-static unsigned int __devinit get_memsize(void)
+static unsigned int __devinit get_memsize(struct tridentfb_par *par)
 {
 	unsigned char tmp, tmp2;
 	unsigned int k;
@@ -737,12 +795,12 @@ static unsigned int __devinit get_memsize(void)
 	if (memsize)
 		k = memsize * Kb;
 	else
-		switch (chip_id) {
+		switch (par->chip_id) {
 		case CYBER9525DVD:
 			k = 2560 * Kb;
 			break;
 		default:
-			tmp = read3X4(SPR) & 0x0F;
+			tmp = read3X4(par, SPR) & 0x0F;
 			switch (tmp) {
 
 			case 0x01:
@@ -774,7 +832,7 @@ static unsigned int __devinit get_memsize(void)
 				break;
 			case 0x0E:		/* XP */
 
-				tmp2 = read3C4(0xC1);
+				tmp2 = vga_mm_rseq(par->io_virt, 0xC1);
 				switch (tmp2) {
 				case 0x00:
 					k = 20 * Mb;
@@ -812,26 +870,67 @@ static unsigned int __devinit get_memsize(void)
 static int tridentfb_check_var(struct fb_var_screeninfo *var,
 			       struct fb_info *info)
 {
+	struct tridentfb_par *par = info->par;
 	int bpp = var->bits_per_pixel;
+	int line_length;
+	int ramdac = 230000; /* 230MHz for most 3D chips */
 	debug("enter\n");
 
 	/* check color depth */
 	if (bpp == 24)
 		bpp = var->bits_per_pixel = 32;
+	if (bpp != 8 && bpp != 16 && bpp != 32)
+		return -EINVAL;
+	if (par->chip_id == TGUI9440 && bpp == 32)
+		return -EINVAL;
 	/* check whether resolution fits on panel and in memory */
-	if (flatpanel && nativex && var->xres > nativex)
+	if (par->flatpanel && nativex && var->xres > nativex)
+		return -EINVAL;
+	/* various resolution checks */
+	var->xres = (var->xres + 7) & ~0x7;
+	if (var->xres > var->xres_virtual)
+		var->xres_virtual = var->xres;
+	if (var->yres > var->yres_virtual)
+		var->yres_virtual = var->yres;
+	if (var->xres_virtual > 4095 || var->yres > 2048)
 		return -EINVAL;
-	if (var->xres * var->yres_virtual * bpp / 8 > info->fix.smem_len)
+	/* prevent from position overflow for acceleration */
+	if (var->yres_virtual > 0xffff)
+		return -EINVAL;
+	line_length = var->xres_virtual * bpp / 8;
+
+	if (!is3Dchip(par->chip_id) &&
+	    !(info->flags & FBINFO_HWACCEL_DISABLED)) {
+		/* acceleration requires line length to be power of 2 */
+		if (line_length <= 512)
+			var->xres_virtual = 512 * 8 / bpp;
+		else if (line_length <= 1024)
+			var->xres_virtual = 1024 * 8 / bpp;
+		else if (line_length <= 2048)
+			var->xres_virtual = 2048 * 8 / bpp;
+		else if (line_length <= 4096)
+			var->xres_virtual = 4096 * 8 / bpp;
+		else if (line_length <= 8192)
+			var->xres_virtual = 8192 * 8 / bpp;
+		else
+			return -EINVAL;
+
+		line_length = var->xres_virtual * bpp / 8;
+	}
+
+	/* datasheet specifies how to set panning only up to 4 MB */
+	if (line_length * (var->yres_virtual - var->yres) > (4 << 20))
+		var->yres_virtual = ((4 << 20) / line_length) + var->yres;
+
+	if (line_length * var->yres_virtual > info->fix.smem_len)
 		return -EINVAL;
 
 	switch (bpp) {
 	case 8:
 		var->red.offset = 0;
-		var->green.offset = 0;
-		var->blue.offset = 0;
-		var->red.length = 6;
-		var->green.length = 6;
-		var->blue.length = 6;
+		var->red.length = 8;
+		var->green = var->red;
+		var->blue = var->red;
 		break;
 	case 16:
 		var->red.offset = 11;
@@ -852,6 +951,33 @@ static int tridentfb_check_var(struct fb_var_screeninfo *var,
 	default:
 		return -EINVAL;
 	}
+
+	if (is_xp(par->chip_id))
+		ramdac = 350000;
+
+	switch (par->chip_id) {
+	case TGUI9440:
+		ramdac = (bpp >= 16) ? 45000 : 90000;
+		break;
+	case CYBER9320:
+	case TGUI9660:
+		ramdac = 135000;
+		break;
+	case PROVIDIA9685:
+	case CYBER9388:
+	case CYBER9382:
+	case CYBER9385:
+		ramdac = 170000;
+		break;
+	}
+
+	/* The clock is doubled for 32 bpp */
+	if (bpp == 32)
+		ramdac /= 2;
+
+	if (PICOS2KHZ(var->pixclock) > ramdac)
+		return -EINVAL;
+
 	debug("exit\n");
 
 	return 0;
@@ -862,25 +988,31 @@ static int tridentfb_check_var(struct fb_var_screeninfo *var,
 static int tridentfb_pan_display(struct fb_var_screeninfo *var,
 				 struct fb_info *info)
 {
+	struct tridentfb_par *par = info->par;
 	unsigned int offset;
 
 	debug("enter\n");
-	offset = (var->xoffset + (var->yoffset * var->xres))
+	offset = (var->xoffset + (var->yoffset * var->xres_virtual))
 		* var->bits_per_pixel / 32;
-	info->var.xoffset = var->xoffset;
-	info->var.yoffset = var->yoffset;
-	set_screen_start(offset);
+	set_screen_start(par, offset);
 	debug("exit\n");
 	return 0;
 }
 
-#define shadowmode_on()  write3CE(CyberControl, read3CE(CyberControl) | 0x81)
-#define shadowmode_off() write3CE(CyberControl, read3CE(CyberControl) & 0x7E)
+static inline void shadowmode_on(struct tridentfb_par *par)
+{
+	write3CE(par, CyberControl, read3CE(par, CyberControl) | 0x81);
+}
+
+static inline void shadowmode_off(struct tridentfb_par *par)
+{
+	write3CE(par, CyberControl, read3CE(par, CyberControl) & 0x7E);
+}
 
 /* Set the hardware to the requested video mode */
 static int tridentfb_set_par(struct fb_info *info)
 {
-	struct tridentfb_par *par = (struct tridentfb_par *)(info->par);
+	struct tridentfb_par *par = info->par;
 	u32 htotal, hdispend, hsyncstart, hsyncend, hblankstart, hblankend;
 	u32 vtotal, vdispend, vsyncstart, vsyncend, vblankstart, vblankend;
 	struct fb_var_screeninfo *var = &info->var;
@@ -891,58 +1023,73 @@ static int tridentfb_set_par(struct fb_info *info)
 	debug("enter\n");
 	hdispend = var->xres / 8 - 1;
 	hsyncstart = (var->xres + var->right_margin) / 8;
-	hsyncend = var->hsync_len / 8;
-	htotal =
-		(var->xres + var->left_margin + var->right_margin +
-		 var->hsync_len) / 8 - 10;
+	hsyncend = (var->xres + var->right_margin + var->hsync_len) / 8;
+	htotal = (var->xres + var->left_margin + var->right_margin +
+		  var->hsync_len) / 8 - 5;
 	hblankstart = hdispend + 1;
-	hblankend = htotal + 5;
+	hblankend = htotal + 3;
 
 	vdispend = var->yres - 1;
 	vsyncstart = var->yres + var->lower_margin;
-	vsyncend = var->vsync_len;
-	vtotal = var->upper_margin + vsyncstart + vsyncend - 2;
-	vblankstart = var->yres;
-	vblankend = vtotal + 2;
+	vsyncend = vsyncstart + var->vsync_len;
+	vtotal = var->upper_margin + vsyncend - 2;
+	vblankstart = vdispend + 1;
+	vblankend = vtotal;
+
+	if (info->var.vmode & FB_VMODE_INTERLACED) {
+		vtotal /= 2;
+		vdispend /= 2;
+		vsyncstart /= 2;
+		vsyncend /= 2;
+		vblankstart /= 2;
+		vblankend /= 2;
+	}
 
-	crtc_unlock();
-	write3CE(CyberControl, 8);
+	enable_mmio(par);
+	crtc_unlock(par);
+	write3CE(par, CyberControl, 8);
+	tmp = 0xEB;
+	if (var->sync & FB_SYNC_HOR_HIGH_ACT)
+		tmp &= ~0x40;
+	if (var->sync & FB_SYNC_VERT_HIGH_ACT)
+		tmp &= ~0x80;
 
-	if (flatpanel && var->xres < nativex) {
+	if (par->flatpanel && var->xres < nativex) {
 		/*
 		 * on flat panels with native size larger
 		 * than requested resolution decide whether
 		 * we stretch or center
 		 */
-		t_outb(0xEB, 0x3C2);
+		t_outb(par, tmp | 0xC0, VGA_MIS_W);
 
-		shadowmode_on();
+		shadowmode_on(par);
 
 		if (center)
-			screen_center();
+			screen_center(par);
 		else if (stretch)
-			screen_stretch();
+			screen_stretch(par);
 
 	} else {
-		t_outb(0x2B, 0x3C2);
-		write3CE(CyberControl, 8);
+		t_outb(par, tmp, VGA_MIS_W);
+		write3CE(par, CyberControl, 8);
 	}
 
 	/* vertical timing values */
-	write3X4(CRTVTotal, vtotal & 0xFF);
-	write3X4(CRTVDispEnd, vdispend & 0xFF);
-	write3X4(CRTVSyncStart, vsyncstart & 0xFF);
-	write3X4(CRTVSyncEnd, (vsyncend & 0x0F));
-	write3X4(CRTVBlankStart, vblankstart & 0xFF);
-	write3X4(CRTVBlankEnd, 0 /* p->vblankend & 0xFF */ );
+	write3X4(par, VGA_CRTC_V_TOTAL, vtotal & 0xFF);
+	write3X4(par, VGA_CRTC_V_DISP_END, vdispend & 0xFF);
+	write3X4(par, VGA_CRTC_V_SYNC_START, vsyncstart & 0xFF);
+	write3X4(par, VGA_CRTC_V_SYNC_END, (vsyncend & 0x0F));
+	write3X4(par, VGA_CRTC_V_BLANK_START, vblankstart & 0xFF);
+	write3X4(par, VGA_CRTC_V_BLANK_END, vblankend & 0xFF);
 
 	/* horizontal timing values */
-	write3X4(CRTHTotal, htotal & 0xFF);
-	write3X4(CRTHDispEnd, hdispend & 0xFF);
-	write3X4(CRTHSyncStart, hsyncstart & 0xFF);
-	write3X4(CRTHSyncEnd, (hsyncend & 0x1F) | ((hblankend & 0x20) << 2));
-	write3X4(CRTHBlankStart, hblankstart & 0xFF);
-	write3X4(CRTHBlankEnd, 0 /* (p->hblankend & 0x1F) */ );
+	write3X4(par, VGA_CRTC_H_TOTAL, htotal & 0xFF);
+	write3X4(par, VGA_CRTC_H_DISP, hdispend & 0xFF);
+	write3X4(par, VGA_CRTC_H_SYNC_START, hsyncstart & 0xFF);
+	write3X4(par, VGA_CRTC_H_SYNC_END,
+		 (hsyncend & 0x1F) | ((hblankend & 0x20) << 2));
+	write3X4(par, VGA_CRTC_H_BLANK_START, hblankstart & 0xFF);
+	write3X4(par, VGA_CRTC_H_BLANK_END, hblankend & 0x1F);
 
 	/* higher bits of vertical timing values */
 	tmp = 0x10;
@@ -954,39 +1101,43 @@ static int tridentfb_set_par(struct fb_info *info)
 	if (vtotal & 0x200) tmp |= 0x20;
 	if (vdispend & 0x200) tmp |= 0x40;
 	if (vsyncstart & 0x200) tmp |= 0x80;
-	write3X4(CRTOverflow, tmp);
+	write3X4(par, VGA_CRTC_OVERFLOW, tmp);
 
-	tmp = read3X4(CRTHiOrd) | 0x08;	/* line compare bit 10 */
+	tmp = read3X4(par, CRTHiOrd) & 0x07;
+	tmp |= 0x08;	/* line compare bit 10 */
 	if (vtotal & 0x400) tmp |= 0x80;
 	if (vblankstart & 0x400) tmp |= 0x40;
 	if (vsyncstart & 0x400) tmp |= 0x20;
 	if (vdispend & 0x400) tmp |= 0x10;
-	write3X4(CRTHiOrd, tmp);
+	write3X4(par, CRTHiOrd, tmp);
 
-	tmp = 0;
-	if (htotal & 0x800) tmp |= 0x800 >> 11;
-	if (hblankstart & 0x800) tmp |= 0x800 >> 7;
-	write3X4(HorizOverflow, tmp);
+	tmp = (htotal >> 8) & 0x01;
+	tmp |= (hdispend >> 7) & 0x02;
+	tmp |= (hsyncstart >> 5) & 0x08;
+	tmp |= (hblankstart >> 4) & 0x10;
+	write3X4(par, HorizOverflow, tmp);
 
 	tmp = 0x40;
 	if (vblankstart & 0x200) tmp |= 0x20;
 //FIXME	if (info->var.vmode & FB_VMODE_DOUBLE) tmp |= 0x80;  /* double scan for 200 line modes */
-	write3X4(CRTMaxScanLine, tmp);
+	write3X4(par, VGA_CRTC_MAX_SCAN, tmp);
 
-	write3X4(CRTLineCompare, 0xFF);
-	write3X4(CRTPRowScan, 0);
-	write3X4(CRTModeControl, 0xC3);
+	write3X4(par, VGA_CRTC_LINE_COMPARE, 0xFF);
+	write3X4(par, VGA_CRTC_PRESET_ROW, 0);
+	write3X4(par, VGA_CRTC_MODE, 0xC3);
 
-	write3X4(LinearAddReg, 0x20);	/* enable linear addressing */
+	write3X4(par, LinearAddReg, 0x20);	/* enable linear addressing */
 
 	tmp = (info->var.vmode & FB_VMODE_INTERLACED) ? 0x84 : 0x80;
-	write3X4(CRTCModuleTest, tmp);	/* enable access extended memory */
-
-	write3X4(GraphEngReg, 0x80);	/* enable GE for text acceleration */
+	/* enable access extended memory */
+	write3X4(par, CRTCModuleTest, tmp);
+	tmp = read3CE(par, MiscIntContReg) & ~0x4;
+	if (info->var.vmode & FB_VMODE_INTERLACED)
+		tmp |= 0x4;
+	write3CE(par, MiscIntContReg, tmp);
 
-#ifdef CONFIG_FB_TRIDENT_ACCEL
-	acc->init_accel(info->var.xres, bpp);
-#endif
+	/* enable GE for text acceleration */
+	write3X4(par, GraphEngReg, 0x80);
 
 	switch (bpp) {
 	case 8:
@@ -1003,57 +1154,59 @@ static int tridentfb_set_par(struct fb_info *info)
 		break;
 	}
 
-	write3X4(PixelBusReg, tmp);
+	write3X4(par, PixelBusReg, tmp);
 
-	tmp = 0x10;
-	if (chipcyber)
+	tmp = read3X4(par, DRAMControl);
+	if (!is_oldprotect(par->chip_id))
+		tmp |= 0x10;
+	if (iscyber(par->chip_id))
 		tmp |= 0x20;
-	write3X4(DRAMControl, tmp);	/* both IO, linear enable */
-
-	write3X4(InterfaceSel, read3X4(InterfaceSel) | 0x40);
-	write3X4(Performance, 0x92);
-	write3X4(PCIReg, 0x07);		/* MMIO & PCI read and write burst enable */
+	write3X4(par, DRAMControl, tmp);	/* both IO, linear enable */
+
+	write3X4(par, InterfaceSel, read3X4(par, InterfaceSel) | 0x40);
+	if (!is_xp(par->chip_id))
+		write3X4(par, Performance, read3X4(par, Performance) | 0x10);
+	/* MMIO & PCI read and write burst enable */
+	if (par->chip_id != TGUI9440 && par->chip_id != IMAGE975)
+		write3X4(par, PCIReg, read3X4(par, PCIReg) | 0x06);
+
+	vga_mm_wseq(par->io_virt, 0, 3);
+	vga_mm_wseq(par->io_virt, 1, 1); /* set char clock 8 dots wide */
+	/* enable 4 maps because needed in chain4 mode */
+	vga_mm_wseq(par->io_virt, 2, 0x0F);
+	vga_mm_wseq(par->io_virt, 3, 0);
+	vga_mm_wseq(par->io_virt, 4, 0x0E); /* memory mode enable bitmaps ?? */
 
 	/* convert from picoseconds to kHz */
 	vclk = PICOS2KHZ(info->var.pixclock);
-	if (bpp == 32)
+
+	/* divide clock by 2 if 32bpp chain4 mode display and CPU path */
+	tmp = read3CE(par, MiscExtFunc) & 0xF0;
+	if (bpp == 32 || (par->chip_id == TGUI9440 && bpp == 16)) {
+		tmp |= 8;
 		vclk *= 2;
-	set_vclk(vclk);
-
-	write3C4(0, 3);
-	write3C4(1, 1);		/* set char clock 8 dots wide */
-	write3C4(2, 0x0F);	/* enable 4 maps because needed in chain4 mode */
-	write3C4(3, 0);
-	write3C4(4, 0x0E);	/* memory mode enable bitmaps ?? */
-
-	write3CE(MiscExtFunc, (bpp == 32) ? 0x1A : 0x12);	/* divide clock by 2 if 32bpp */
-							/* chain4 mode display and CPU path */
-	write3CE(0x5, 0x40);	/* no CGA compat, allow 256 col */
-	write3CE(0x6, 0x05);	/* graphics mode */
-	write3CE(0x7, 0x0F);	/* planes? */
-
-	if (chip_id == CYBERBLADEXPAi1) {
-		/* This fixes snow-effect in 32 bpp */
-		write3X4(CRTHSyncStart, 0x84);
 	}
+	set_vclk(par, vclk);
+	write3CE(par, MiscExtFunc, tmp | 0x12);
+	write3CE(par, 0x5, 0x40);	/* no CGA compat, allow 256 col */
+	write3CE(par, 0x6, 0x05);	/* graphics mode */
+	write3CE(par, 0x7, 0x0F);	/* planes? */
 
-	writeAttr(0x10, 0x41);	/* graphics mode and support 256 color modes */
-	writeAttr(0x12, 0x0F);	/* planes */
-	writeAttr(0x13, 0);	/* horizontal pel panning */
+	/* graphics mode and support 256 color modes */
+	writeAttr(par, 0x10, 0x41);
+	writeAttr(par, 0x12, 0x0F);	/* planes */
+	writeAttr(par, 0x13, 0);	/* horizontal pel panning */
 
 	/* colors */
 	for (tmp = 0; tmp < 0x10; tmp++)
-		writeAttr(tmp, tmp);
-	readb(par->io_virt + CRT + 0x0A);	/* flip-flop to index */
-	t_outb(0x20, 0x3C0);			/* enable attr */
+		writeAttr(par, tmp, tmp);
+	fb_readb(par->io_virt + VGA_IS1_RC);	/* flip-flop to index */
+	t_outb(par, 0x20, VGA_ATT_W);		/* enable attr */
 
 	switch (bpp) {
 	case 8:
 		tmp = 0;
 		break;
-	case 15:
-		tmp = 0x10;
-		break;
 	case 16:
 		tmp = 0x30;
 		break;
@@ -1063,19 +1216,23 @@ static int tridentfb_set_par(struct fb_info *info)
 		break;
 	}
 
-	t_inb(0x3C8);
-	t_inb(0x3C6);
-	t_inb(0x3C6);
-	t_inb(0x3C6);
-	t_inb(0x3C6);
-	t_outb(tmp, 0x3C6);
-	t_inb(0x3C8);
-
-	if (flatpanel)
-		set_number_of_lines(info->var.yres);
-	set_lwidth(info->var.xres * bpp / (4 * 16));
+	t_inb(par, VGA_PEL_IW);
+	t_inb(par, VGA_PEL_MSK);
+	t_inb(par, VGA_PEL_MSK);
+	t_inb(par, VGA_PEL_MSK);
+	t_inb(par, VGA_PEL_MSK);
+	t_outb(par, tmp, VGA_PEL_MSK);
+	t_inb(par, VGA_PEL_IW);
+
+	if (par->flatpanel)
+		set_number_of_lines(par, info->var.yres);
+	info->fix.line_length = info->var.xres_virtual * bpp / 8;
+	set_lwidth(par, info->fix.line_length / 8);
+
+	if (!(info->flags & FBINFO_HWACCEL_DISABLED))
+		par->init_accel(par, info->var.xres_virtual, bpp);
+
 	info->fix.visual = (bpp == 8) ? FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
-	info->fix.line_length = info->var.xres * (bpp >> 3);
 	info->cmap.len = (bpp == 8) ? 256 : 16;
 	debug("exit\n");
 	return 0;
@@ -1087,17 +1244,18 @@ static int tridentfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			       struct fb_info *info)
 {
 	int bpp = info->var.bits_per_pixel;
+	struct tridentfb_par *par = info->par;
 
 	if (regno >= info->cmap.len)
 		return 1;
 
 	if (bpp == 8) {
-		t_outb(0xFF, 0x3C6);
-		t_outb(regno, 0x3C8);
+		t_outb(par, 0xFF, VGA_PEL_MSK);
+		t_outb(par, regno, VGA_PEL_IW);
 
-		t_outb(red >> 10, 0x3C9);
-		t_outb(green >> 10, 0x3C9);
-		t_outb(blue >> 10, 0x3C9);
+		t_outb(par, red >> 10, VGA_PEL_D);
+		t_outb(par, green >> 10, VGA_PEL_D);
+		t_outb(par, blue >> 10, VGA_PEL_D);
 
 	} else if (regno < 16) {
 		if (bpp == 16) {	/* RGB 565 */
@@ -1108,28 +1266,28 @@ static int tridentfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			col |= col << 16;
 			((u32 *)(info->pseudo_palette))[regno] = col;
 		} else if (bpp == 32)		/* ARGB 8888 */
-			((u32*)info->pseudo_palette)[regno] =
+			((u32 *)info->pseudo_palette)[regno] =
 				((transp & 0xFF00) << 16)	|
 				((red & 0xFF00) << 8)		|
 				((green & 0xFF00))		|
 				((blue & 0xFF00) >> 8);
 	}
 
-/* 	debug("exit\n"); */
 	return 0;
 }
 
-/* Try blanking the screen.For flat panels it does nothing */
+/* Try blanking the screen. For flat panels it does nothing */
 static int tridentfb_blank(int blank_mode, struct fb_info *info)
 {
 	unsigned char PMCont, DPMSCont;
+	struct tridentfb_par *par = info->par;
 
 	debug("enter\n");
-	if (flatpanel)
+	if (par->flatpanel)
 		return 0;
-	t_outb(0x04, 0x83C8); /* Read DPMS Control */
-	PMCont = t_inb(0x83C6) & 0xFC;
-	DPMSCont = read3CE(PowerStatus) & 0xFC;
+	t_outb(par, 0x04, 0x83C8); /* Read DPMS Control */
+	PMCont = t_inb(par, 0x83C6) & 0xFC;
+	DPMSCont = read3CE(par, PowerStatus) & 0xFC;
 	switch (blank_mode) {
 	case FB_BLANK_UNBLANK:
 		/* Screen: On, HSync: On, VSync: On */
@@ -1155,9 +1313,9 @@ static int tridentfb_blank(int blank_mode, struct fb_info *info)
 		break;
 	}
 
-	write3CE(PowerStatus, DPMSCont);
-	t_outb(4, 0x83C8);
-	t_outb(PMCont, 0x83C6);
+	write3CE(par, PowerStatus, DPMSCont);
+	t_outb(par, 4, 0x83C8);
+	t_outb(par, PMCont, 0x83C6);
 
 	debug("exit\n");
 
@@ -1174,33 +1332,46 @@ static struct fb_ops tridentfb_ops = {
 	.fb_set_par = tridentfb_set_par,
 	.fb_fillrect = tridentfb_fillrect,
 	.fb_copyarea = tridentfb_copyarea,
-	.fb_imageblit = cfb_imageblit,
+	.fb_imageblit = tridentfb_imageblit,
+#ifdef CONFIG_FB_TRIDENT_ACCEL
+	.fb_sync = tridentfb_sync,
+#endif
 };
 
-static int __devinit trident_pci_probe(struct pci_dev * dev,
-				       const struct pci_device_id * id)
+static int __devinit trident_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *id)
 {
 	int err;
 	unsigned char revision;
+	struct fb_info *info;
+	struct tridentfb_par *default_par;
+	int chip3D;
+	int chip_id;
 
 	err = pci_enable_device(dev);
 	if (err)
 		return err;
 
-	chip_id = id->device;
+	info = framebuffer_alloc(sizeof(struct tridentfb_par), &dev->dev);
+	if (!info)
+		return -ENOMEM;
+	default_par = info->par;
 
-	if (chip_id == CYBERBLADEi1)
-		output("*** Please do use cyblafb, Cyberblade/i1 support "
-		       "will soon be removed from tridentfb!\n");
+	chip_id = id->device;
 
+#ifndef CONFIG_FB_TRIDENT_ACCEL
+	noaccel = 1;
+#endif
 
 	/* If PCI id is 0x9660 then further detect chip type */
 
 	if (chip_id == TGUI9660) {
-		outb(RevisionID, 0x3C4);
-		revision = inb(0x3C5);
+		revision = vga_io_rseq(RevisionID);
 
 		switch (revision) {
+		case 0x21:
+			chip_id = PROVIDIA9685;
+			break;
 		case 0x22:
 		case 0x23:
 			chip_id = CYBER9397;
@@ -1229,123 +1400,170 @@ static int __devinit trident_pci_probe(struct pci_dev * dev,
 	}
 
 	chip3D = is3Dchip(chip_id);
-	chipcyber = iscyber(chip_id);
 
 	if (is_xp(chip_id)) {
-		acc = &accel_xp;
+		default_par->init_accel = xp_init_accel;
+		default_par->wait_engine = xp_wait_engine;
+		default_par->fill_rect = xp_fill_rect;
+		default_par->copy_rect = xp_copy_rect;
+		tridentfb_fix.accel = FB_ACCEL_TRIDENT_BLADEXP;
 	} else if (is_blade(chip_id)) {
-		acc = &accel_blade;
-	} else {
-		acc = &accel_image;
+		default_par->init_accel = blade_init_accel;
+		default_par->wait_engine = blade_wait_engine;
+		default_par->fill_rect = blade_fill_rect;
+		default_par->copy_rect = blade_copy_rect;
+		default_par->image_blit = blade_image_blit;
+		tridentfb_fix.accel = FB_ACCEL_TRIDENT_BLADE3D;
+	} else if (chip3D) {			/* 3DImage family left */
+		default_par->init_accel = image_init_accel;
+		default_par->wait_engine = image_wait_engine;
+		default_par->fill_rect = image_fill_rect;
+		default_par->copy_rect = image_copy_rect;
+		tridentfb_fix.accel = FB_ACCEL_TRIDENT_3DIMAGE;
+	} else { 				/* TGUI 9440/96XX family */
+		default_par->init_accel = tgui_init_accel;
+		default_par->wait_engine = xp_wait_engine;
+		default_par->fill_rect = tgui_fill_rect;
+		default_par->copy_rect = tgui_copy_rect;
+		tridentfb_fix.accel = FB_ACCEL_TRIDENT_TGUI;
 	}
 
-	/* acceleration is on by default for 3D chips */
-	defaultaccel = chip3D && !noaccel;
-
-	fb_info.par = &default_par;
+	default_par->chip_id = chip_id;
 
 	/* setup MMIO region */
 	tridentfb_fix.mmio_start = pci_resource_start(dev, 1);
-	tridentfb_fix.mmio_len = chip3D ? 0x20000 : 0x10000;
+	tridentfb_fix.mmio_len = pci_resource_len(dev, 1);
 
-	if (!request_mem_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len, "tridentfb")) {
+	if (!request_mem_region(tridentfb_fix.mmio_start,
+				tridentfb_fix.mmio_len, "tridentfb")) {
 		debug("request_region failed!\n");
+		framebuffer_release(info);
 		return -1;
 	}
 
-	default_par.io_virt = ioremap_nocache(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
+	default_par->io_virt = ioremap_nocache(tridentfb_fix.mmio_start,
+					       tridentfb_fix.mmio_len);
 
-	if (!default_par.io_virt) {
+	if (!default_par->io_virt) {
 		debug("ioremap failed\n");
 		err = -1;
 		goto out_unmap1;
 	}
 
-	enable_mmio();
+	enable_mmio(default_par);
 
 	/* setup framebuffer memory */
 	tridentfb_fix.smem_start = pci_resource_start(dev, 0);
-	tridentfb_fix.smem_len = get_memsize();
+	tridentfb_fix.smem_len = get_memsize(default_par);
 
-	if (!request_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len, "tridentfb")) {
+	if (!request_mem_region(tridentfb_fix.smem_start,
+				tridentfb_fix.smem_len, "tridentfb")) {
 		debug("request_mem_region failed!\n");
-		disable_mmio();
+		disable_mmio(info->par);
 		err = -1;
 		goto out_unmap1;
 	}
 
-	fb_info.screen_base = ioremap_nocache(tridentfb_fix.smem_start,
-					      tridentfb_fix.smem_len);
+	info->screen_base = ioremap_nocache(tridentfb_fix.smem_start,
+					    tridentfb_fix.smem_len);
 
-	if (!fb_info.screen_base) {
+	if (!info->screen_base) {
 		debug("ioremap failed\n");
 		err = -1;
 		goto out_unmap2;
 	}
 
-	output("%s board found\n", pci_name(dev));
-	displaytype = get_displaytype();
+	default_par->flatpanel = is_flatpanel(default_par);
 
-	if (flatpanel)
-		nativex = get_nativex();
+	if (default_par->flatpanel)
+		nativex = get_nativex(default_par);
 
-	fb_info.fix = tridentfb_fix;
-	fb_info.fbops = &tridentfb_ops;
+	info->fix = tridentfb_fix;
+	info->fbops = &tridentfb_ops;
+	info->pseudo_palette = default_par->pseudo_pal;
 
+	info->flags = FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
+	if (!noaccel && default_par->init_accel) {
+		info->flags &= ~FBINFO_HWACCEL_DISABLED;
+		info->flags |= FBINFO_HWACCEL_COPYAREA;
+		info->flags |= FBINFO_HWACCEL_FILLRECT;
+	} else
+		info->flags |= FBINFO_HWACCEL_DISABLED;
 
-	fb_info.flags = FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
-#ifdef CONFIG_FB_TRIDENT_ACCEL
-	fb_info.flags |= FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT;
-#endif
-	fb_info.pseudo_palette = pseudo_pal;
+	info->pixmap.addr = kmalloc(4096, GFP_KERNEL);
+	if (!info->pixmap.addr) {
+		err = -ENOMEM;
+		goto out_unmap2;
+	}
+
+	info->pixmap.size = 4096;
+	info->pixmap.buf_align = 4;
+	info->pixmap.scan_align = 1;
+	info->pixmap.access_align = 32;
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
 
-	if (!fb_find_mode(&default_var, &fb_info,
+	if (default_par->image_blit) {
+		info->flags |= FBINFO_HWACCEL_IMAGEBLIT;
+		info->pixmap.scan_align = 4;
+	}
+
+	if (noaccel) {
+		printk(KERN_DEBUG "disabling acceleration\n");
+		info->flags |= FBINFO_HWACCEL_DISABLED;
+		info->pixmap.scan_align = 1;
+	}
+
+	if (!fb_find_mode(&info->var, info,
 			  mode_option, NULL, 0, NULL, bpp)) {
 		err = -EINVAL;
 		goto out_unmap2;
 	}
-	err = fb_alloc_cmap(&fb_info.cmap, 256, 0);
+	err = fb_alloc_cmap(&info->cmap, 256, 0);
 	if (err < 0)
 		goto out_unmap2;
 
-	if (defaultaccel && acc)
-		default_var.accel_flags |= FB_ACCELF_TEXT;
-	else
-		default_var.accel_flags &= ~FB_ACCELF_TEXT;
-	default_var.activate |= FB_ACTIVATE_NOW;
-	fb_info.var = default_var;
-	fb_info.device = &dev->dev;
-	if (register_framebuffer(&fb_info) < 0) {
-		printk(KERN_ERR "tridentfb: could not register Trident framebuffer\n");
-		fb_dealloc_cmap(&fb_info.cmap);
+	info->var.activate |= FB_ACTIVATE_NOW;
+	info->device = &dev->dev;
+	if (register_framebuffer(info) < 0) {
+		printk(KERN_ERR "tridentfb: could not register framebuffer\n");
+		fb_dealloc_cmap(&info->cmap);
 		err = -EINVAL;
 		goto out_unmap2;
 	}
 	output("fb%d: %s frame buffer device %dx%d-%dbpp\n",
-	   fb_info.node, fb_info.fix.id, default_var.xres,
-	   default_var.yres, default_var.bits_per_pixel);
+	   info->node, info->fix.id, info->var.xres,
+	   info->var.yres, info->var.bits_per_pixel);
+
+	pci_set_drvdata(dev, info);
 	return 0;
 
 out_unmap2:
-	if (fb_info.screen_base)
-		iounmap(fb_info.screen_base);
+	kfree(info->pixmap.addr);
+	if (info->screen_base)
+		iounmap(info->screen_base);
 	release_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len);
-	disable_mmio();
+	disable_mmio(info->par);
 out_unmap1:
-	if (default_par.io_virt)
-		iounmap(default_par.io_virt);
+	if (default_par->io_virt)
+		iounmap(default_par->io_virt);
 	release_mem_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
+	framebuffer_release(info);
 	return err;
 }
 
 static void __devexit trident_pci_remove(struct pci_dev *dev)
 {
-	struct tridentfb_par *par = (struct tridentfb_par*)fb_info.par;
-	unregister_framebuffer(&fb_info);
+	struct fb_info *info = pci_get_drvdata(dev);
+	struct tridentfb_par *par = info->par;
+
+	unregister_framebuffer(info);
 	iounmap(par->io_virt);
-	iounmap(fb_info.screen_base);
+	iounmap(info->screen_base);
 	release_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len);
 	release_mem_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
+	pci_set_drvdata(dev, NULL);
+	kfree(info->pixmap.addr);
+	framebuffer_release(info);
 }
 
 /* List of boards that we are trying to support */
@@ -1358,6 +1576,7 @@ static struct pci_device_id trident_devices[] = {
 	{PCI_VENDOR_ID_TRIDENT,	CYBERBLADEAi1, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{PCI_VENDOR_ID_TRIDENT,	CYBERBLADEAi1D, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{PCI_VENDOR_ID_TRIDENT,	CYBERBLADEE4, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_TRIDENT,	TGUI9440, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{PCI_VENDOR_ID_TRIDENT,	TGUI9660, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{PCI_VENDOR_ID_TRIDENT,	IMAGE975, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{PCI_VENDOR_ID_TRIDENT,	IMAGE985, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
@@ -1399,9 +1618,9 @@ static int __init tridentfb_setup(char *options)
 		if (!strncmp(opt, "noaccel", 7))
 			noaccel = 1;
 		else if (!strncmp(opt, "fp", 2))
-			displaytype = DISPLAY_FP;
+			fp = 1;
 		else if (!strncmp(opt, "crt", 3))
-			displaytype = DISPLAY_CRT;
+			fp = 0;
 		else if (!strncmp(opt, "bpp=", 4))
 			bpp = simple_strtoul(opt + 4, NULL, 0);
 		else if (!strncmp(opt, "center", 6))
@@ -1430,7 +1649,6 @@ static int __init tridentfb_init(void)
 		return -ENODEV;
 	tridentfb_setup(option);
 #endif
-	output("Trident framebuffer %s initializing\n", VERSION);
 	return pci_register_driver(&tridentfb_pci_driver);
 }
 
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index cdbb56edb6c..50744229c7a 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -2054,8 +2054,8 @@ MODULE_PARM_DESC(maxhf,
 module_param(maxvf, ushort, 0);
 MODULE_PARM_DESC(maxvf,
 	"Maximum vertical frequency [Hz], overrides EDID data");
-module_param_named(mode, mode_option, charp, 0);
-MODULE_PARM_DESC(mode,
+module_param(mode_option, charp, 0);
+MODULE_PARM_DESC(mode_option,
 	"Specify initial video mode as \"<xres>x<yres>[-<bpp>][@<refresh>]\"");
 module_param(vbemode, ushort, 0);
 MODULE_PARM_DESC(vbemode,
diff --git a/drivers/video/vfb.c b/drivers/video/vfb.c
index 072638a9528..93fe08d6c78 100644
--- a/drivers/video/vfb.c
+++ b/drivers/video/vfb.c
@@ -443,19 +443,29 @@ static int vfb_mmap(struct fb_info *info,
 }
 
 #ifndef MODULE
+/*
+ * The virtual framebuffer driver is only enabled if explicitly
+ * requested by passing 'video=vfb:' (or any actual options).
+ */
 static int __init vfb_setup(char *options)
 {
 	char *this_opt;
 
+	vfb_enable = 0;
+
+	if (!options)
+		return 1;
+
 	vfb_enable = 1;
 
-	if (!options || !*options)
+	if (!*options)
 		return 1;
 
 	while ((this_opt = strsep(&options, ",")) != NULL) {
 		if (!*this_opt)
 			continue;
-		if (!strncmp(this_opt, "disable", 7))
+		/* Test disable for backwards compatibility */
+		if (!strcmp(this_opt, "disable"))
 			vfb_enable = 0;
 	}
 	return 1;
diff --git a/drivers/video/vga16fb.c b/drivers/video/vga16fb.c
index 9b3c5923365..e31bca8a0cb 100644
--- a/drivers/video/vga16fb.c
+++ b/drivers/video/vga16fb.c
@@ -26,18 +26,6 @@
 #include <asm/io.h>
 #include <video/vga.h>
 
-#define GRAPHICS_ADDR_REG VGA_GFX_I	/* Graphics address register. */
-#define GRAPHICS_DATA_REG VGA_GFX_D	/* Graphics data register. */
-
-#define SET_RESET_INDEX 	VGA_GFX_SR_VALUE	/* Set/Reset Register index. */
-#define ENABLE_SET_RESET_INDEX	VGA_GFX_SR_ENABLE	/* Enable Set/Reset Register index. */
-#define DATA_ROTATE_INDEX	VGA_GFX_DATA_ROTATE	/* Data Rotate Register index. */
-#define GRAPHICS_MODE_INDEX	VGA_GFX_MODE		/* Graphics Mode Register index. */
-#define BIT_MASK_INDEX		VGA_GFX_BIT_MASK	/* Bit Mask Register index. */
-
-#define dac_reg	(VGA_PEL_IW)
-#define dac_val	(VGA_PEL_D)
-
 #define VGA_FB_PHYS 0xA0000
 #define VGA_FB_PHYS_LEN 65536
 
@@ -108,7 +96,7 @@ static struct fb_fix_screeninfo vga16fb_fix __initdata = {
 	.visual		= FB_VISUAL_PSEUDOCOLOR,
 	.xpanstep	= 8,
 	.ypanstep	= 1,
-	.line_length	= 640/8,
+	.line_length	= 640 / 8,
 	.accel		= FB_ACCEL_NONE
 };
 
@@ -135,23 +123,22 @@ static inline int setmode(int mode)
 {
 	int oldmode;
 	
-	vga_io_w(GRAPHICS_ADDR_REG, GRAPHICS_MODE_INDEX);
-	oldmode = vga_io_r(GRAPHICS_DATA_REG);
-	vga_io_w(GRAPHICS_DATA_REG, mode);
+	oldmode = vga_io_rgfx(VGA_GFX_MODE);
+	vga_io_w(VGA_GFX_D, mode);
 	return oldmode;
 }
 
 /* Select the Bit Mask Register and return its value. */
 static inline int selectmask(void)
 {
-	return vga_io_rgfx(BIT_MASK_INDEX);
+	return vga_io_rgfx(VGA_GFX_BIT_MASK);
 }
 
 /* Set the value of the Bit Mask Register.  It must already have been
    selected with selectmask(). */
 static inline void setmask(int mask)
 {
-	vga_io_w(GRAPHICS_DATA_REG, mask);
+	vga_io_w(VGA_GFX_D, mask);
 }
 
 /* Set the Data Rotate Register and return its old value. 
@@ -161,9 +148,8 @@ static inline int setop(int op)
 {
 	int oldop;
 	
-	vga_io_w(GRAPHICS_ADDR_REG, DATA_ROTATE_INDEX);
-	oldop = vga_io_r(GRAPHICS_DATA_REG);
-	vga_io_w(GRAPHICS_DATA_REG, op);
+	oldop = vga_io_rgfx(VGA_GFX_DATA_ROTATE);
+	vga_io_w(VGA_GFX_D, op);
 	return oldop;
 }
 
@@ -173,9 +159,8 @@ static inline int setsr(int sr)
 {
 	int oldsr;
 
-	vga_io_w(GRAPHICS_ADDR_REG, ENABLE_SET_RESET_INDEX);
-	oldsr = vga_io_r(GRAPHICS_DATA_REG);
-	vga_io_w(GRAPHICS_DATA_REG, sr);
+	oldsr = vga_io_rgfx(VGA_GFX_SR_ENABLE);
+	vga_io_w(VGA_GFX_D, sr);
 	return oldsr;
 }
 
@@ -184,22 +169,21 @@ static inline int setcolor(int color)
 {
 	int oldcolor;
 
-	vga_io_w(GRAPHICS_ADDR_REG, SET_RESET_INDEX);
-	oldcolor = vga_io_r(GRAPHICS_DATA_REG);
-	vga_io_w(GRAPHICS_DATA_REG, color);
+	oldcolor = vga_io_rgfx(VGA_GFX_SR_VALUE);
+	vga_io_w(VGA_GFX_D, color);
 	return oldcolor;
 }
 
 /* Return the value in the Graphics Address Register. */
 static inline int getindex(void)
 {
-	return vga_io_r(GRAPHICS_ADDR_REG);
+	return vga_io_r(VGA_GFX_I);
 }
 
 /* Set the value in the Graphics Address Register. */
 static inline void setindex(int index)
 {
-	vga_io_w(GRAPHICS_ADDR_REG, index);
+	vga_io_w(VGA_GFX_I, index);
 }
 
 static void vga16fb_pan_var(struct fb_info *info, 
@@ -672,10 +656,10 @@ static void ega16_setpalette(int regno, unsigned red, unsigned green, unsigned b
 
 static void vga16_setpalette(int regno, unsigned red, unsigned green, unsigned blue)
 {
-	outb(regno,       dac_reg);
-	outb(red   >> 10, dac_val);
-	outb(green >> 10, dac_val);
-	outb(blue  >> 10, dac_val);
+	outb(regno,       VGA_PEL_IW);
+	outb(red   >> 10, VGA_PEL_D);
+	outb(green >> 10, VGA_PEL_D);
+	outb(blue  >> 10, VGA_PEL_D);
 }
 
 static int vga16fb_setcolreg(unsigned regno, unsigned red, unsigned green,
@@ -719,28 +703,15 @@ static int vga16fb_pan_display(struct fb_var_screeninfo *var,
    blanking code was originally by Huang shi chao, and modified by
    Christoph Rimek (chrimek@toppoint.de) and todd j. derr
    (tjd@barefoot.org) for Linux. */
-#define attrib_port		VGA_ATC_IW
-#define seq_port_reg		VGA_SEQ_I
-#define seq_port_val		VGA_SEQ_D
-#define gr_port_reg		VGA_GFX_I
-#define gr_port_val		VGA_GFX_D
-#define video_misc_rd		VGA_MIS_R
-#define video_misc_wr		VGA_MIS_W
-#define vga_video_port_reg	VGA_CRT_IC
-#define vga_video_port_val	VGA_CRT_DC
 
 static void vga_vesa_blank(struct vga16fb_par *par, int mode)
 {
-	unsigned char SeqCtrlIndex;
-	unsigned char CrtCtrlIndex;
+	unsigned char SeqCtrlIndex = vga_io_r(VGA_SEQ_I);
+	unsigned char CrtCtrlIndex = vga_io_r(VGA_CRT_IC);
 	
-	//cli();
-	SeqCtrlIndex = vga_io_r(seq_port_reg);
-	CrtCtrlIndex = vga_io_r(vga_video_port_reg);
-
 	/* save original values of VGA controller registers */
 	if(!par->vesa_blanked) {
-		par->vga_state.CrtMiscIO = vga_io_r(video_misc_rd);
+		par->vga_state.CrtMiscIO = vga_io_r(VGA_MIS_R);
 		//sti();
 
 		par->vga_state.HorizontalTotal = vga_io_rcrt(0x00);	/* HorizontalTotal */
@@ -756,12 +727,11 @@ static void vga_vesa_blank(struct vga16fb_par *par, int mode)
 
 	/* assure that video is enabled */
 	/* "0x20" is VIDEO_ENABLE_bit in register 01 of sequencer */
-	//cli();
 	vga_io_wseq(0x01, par->vga_state.ClockingMode | 0x20);
 
 	/* test for vertical retrace in process.... */
 	if ((par->vga_state.CrtMiscIO & 0x80) == 0x80)
-		vga_io_w(video_misc_wr, par->vga_state.CrtMiscIO & 0xef);
+		vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO & 0xef);
 
 	/*
 	 * Set <End of vertical retrace> to minimum (0) and
@@ -769,12 +739,10 @@ static void vga_vesa_blank(struct vga16fb_par *par, int mode)
 	 * Result: turn off vertical sync (VSync) pulse.
 	 */
 	if (mode & FB_BLANK_VSYNC_SUSPEND) {
-		outb_p(0x10,vga_video_port_reg);	/* StartVertRetrace */
-		outb_p(0xff,vga_video_port_val); 	/* maximum value */
-		outb_p(0x11,vga_video_port_reg);	/* EndVertRetrace */
-		outb_p(0x40,vga_video_port_val);	/* minimum (bits 0..3)  */
-		outb_p(0x07,vga_video_port_reg);	/* Overflow */
-		outb_p(par->vga_state.Overflow | 0x84,vga_video_port_val); /* bits 9,10 of vert. retrace */
+		vga_io_wcrt(VGA_CRTC_V_SYNC_START, 0xff);
+		vga_io_wcrt(VGA_CRTC_V_SYNC_END, 0x40);
+		/* bits 9,10 of vert. retrace */
+		vga_io_wcrt(VGA_CRTC_OVERFLOW, par->vga_state.Overflow | 0x84);
 	}
 
 	if (mode & FB_BLANK_HSYNC_SUSPEND) {
@@ -783,29 +751,22 @@ static void vga_vesa_blank(struct vga16fb_par *par, int mode)
 		 *  <Start of horizontal Retrace> to maximum
 		 * Result: turn off horizontal sync (HSync) pulse.
 		 */
-		outb_p(0x04,vga_video_port_reg);	/* StartHorizRetrace */
-		outb_p(0xff,vga_video_port_val);	/* maximum */
-		outb_p(0x05,vga_video_port_reg);	/* EndHorizRetrace */
-		outb_p(0x00,vga_video_port_val);	/* minimum (0) */
+		vga_io_wcrt(VGA_CRTC_H_SYNC_START, 0xff);
+		vga_io_wcrt(VGA_CRTC_H_SYNC_END, 0x00);
 	}
 
 	/* restore both index registers */
-	outb_p(SeqCtrlIndex,seq_port_reg);
-	outb_p(CrtCtrlIndex,vga_video_port_reg);
-	//sti();
+	outb_p(SeqCtrlIndex, VGA_SEQ_I);
+	outb_p(CrtCtrlIndex, VGA_CRT_IC);
 }
 
 static void vga_vesa_unblank(struct vga16fb_par *par)
 {
-	unsigned char SeqCtrlIndex;
-	unsigned char CrtCtrlIndex;
+	unsigned char SeqCtrlIndex = vga_io_r(VGA_SEQ_I);
+	unsigned char CrtCtrlIndex = vga_io_r(VGA_CRT_IC);
 	
-	//cli();
-	SeqCtrlIndex = vga_io_r(seq_port_reg);
-	CrtCtrlIndex = vga_io_r(vga_video_port_reg);
-
 	/* restore original values of VGA controller registers */
-	vga_io_w(video_misc_wr, par->vga_state.CrtMiscIO);
+	vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO);
 
 	/* HorizontalTotal */
 	vga_io_wcrt(0x00, par->vga_state.HorizontalTotal);
@@ -827,9 +788,8 @@ static void vga_vesa_unblank(struct vga16fb_par *par)
 	vga_io_wseq(0x01, par->vga_state.ClockingMode);
 
 	/* restore index/control registers */
-	vga_io_w(seq_port_reg, SeqCtrlIndex);
-	vga_io_w(vga_video_port_reg, CrtCtrlIndex);
-	//sti();
+	vga_io_w(VGA_SEQ_I, SeqCtrlIndex);
+	vga_io_w(VGA_CRT_IC, CrtCtrlIndex);
 }
 
 static void vga_pal_blank(void)
@@ -837,10 +797,10 @@ static void vga_pal_blank(void)
 	int i;
 
 	for (i=0; i<16; i++) {
-		outb_p (i, dac_reg) ;
-		outb_p (0, dac_val) ;
-		outb_p (0, dac_val) ;
-		outb_p (0, dac_val) ;
+		outb_p(i, VGA_PEL_IW);
+		outb_p(0, VGA_PEL_D);
+		outb_p(0, VGA_PEL_D);
+		outb_p(0, VGA_PEL_D);
 	}
 }
 
@@ -1087,12 +1047,15 @@ static void vga16fb_copyarea(struct fb_info *info, const struct fb_copyarea *are
 	width = x2 - dx;
 	height = y2 - dy;
 
+	if (sx + dx < old_dx || sy + dy < old_dy)
+		return;
+
 	/* update sx1,sy1 */
 	sx += (dx - old_dx);
 	sy += (dy - old_dy);
 
 	/* the source must be completely inside the virtual screen */
-	if (sx < 0 || sy < 0 || (sx + width) > vxres || (sy + height) > vyres)
+	if (sx + width > vxres || sy + height > vyres)
 		return;
 
 	switch (info->fix.type) {
@@ -1482,6 +1445,7 @@ static void __exit vga16fb_exit(void)
 	platform_driver_unregister(&vga16fb_driver);
 }
 
+MODULE_DESCRIPTION("Legacy VGA framebuffer device driver");
 MODULE_LICENSE("GPL");
 module_init(vga16fb_init);
 module_exit(vga16fb_exit);
diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index 7084e7e146c..5b78fd0aff0 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -71,13 +71,6 @@ static int virtio_uevent(struct device *_dv, struct kobj_uevent_env *env)
 			      dev->id.device, dev->id.vendor);
 }
 
-static struct bus_type virtio_bus = {
-	.name  = "virtio",
-	.match = virtio_dev_match,
-	.dev_attrs = virtio_dev_attrs,
-	.uevent = virtio_uevent,
-};
-
 static void add_status(struct virtio_device *dev, unsigned status)
 {
 	dev->config->set_status(dev, dev->config->get_status(dev) | status);
@@ -120,12 +113,16 @@ static int virtio_dev_probe(struct device *_d)
 			set_bit(f, dev->features);
 	}
 
+	/* Transport features always preserved to pass to finalize_features. */
+	for (i = VIRTIO_TRANSPORT_F_START; i < VIRTIO_TRANSPORT_F_END; i++)
+		if (device_features & (1 << i))
+			set_bit(i, dev->features);
+
 	err = drv->probe(dev);
 	if (err)
 		add_status(dev, VIRTIO_CONFIG_S_FAILED);
 	else {
-		/* They should never have set feature bits beyond 32 */
-		dev->config->set_features(dev, dev->features[0]);
+		dev->config->finalize_features(dev);
 		add_status(dev, VIRTIO_CONFIG_S_DRIVER_OK);
 	}
 	return err;
@@ -147,13 +144,20 @@ static int virtio_dev_remove(struct device *_d)
 	return 0;
 }
 
+static struct bus_type virtio_bus = {
+	.name  = "virtio",
+	.match = virtio_dev_match,
+	.dev_attrs = virtio_dev_attrs,
+	.uevent = virtio_uevent,
+	.probe = virtio_dev_probe,
+	.remove = virtio_dev_remove,
+};
+
 int register_virtio_driver(struct virtio_driver *driver)
 {
 	/* Catch this early. */
 	BUG_ON(driver->feature_table_size && !driver->feature_table);
 	driver->driver.bus = &virtio_bus;
-	driver->driver.probe = virtio_dev_probe;
-	driver->driver.remove = virtio_dev_remove;
 	return driver_register(&driver->driver);
 }
 EXPORT_SYMBOL_GPL(register_virtio_driver);
diff --git a/drivers/virtio/virtio_pci.c b/drivers/virtio/virtio_pci.c
index eae7236310e..c7dc37c7cce 100644
--- a/drivers/virtio/virtio_pci.c
+++ b/drivers/virtio/virtio_pci.c
@@ -94,12 +94,17 @@ static u32 vp_get_features(struct virtio_device *vdev)
 	return ioread32(vp_dev->ioaddr + VIRTIO_PCI_HOST_FEATURES);
 }
 
-/* virtio config->set_features() implementation */
-static void vp_set_features(struct virtio_device *vdev, u32 features)
+/* virtio config->finalize_features() implementation */
+static void vp_finalize_features(struct virtio_device *vdev)
 {
 	struct virtio_pci_device *vp_dev = to_vp_device(vdev);
 
-	iowrite32(features, vp_dev->ioaddr + VIRTIO_PCI_GUEST_FEATURES);
+	/* Give virtio_ring a chance to accept features. */
+	vring_transport_features(vdev);
+
+	/* We only support 32 feature bits. */
+	BUILD_BUG_ON(ARRAY_SIZE(vdev->features) != 1);
+	iowrite32(vdev->features[0], vp_dev->ioaddr+VIRTIO_PCI_GUEST_FEATURES);
 }
 
 /* virtio config->get() implementation */
@@ -297,7 +302,7 @@ static struct virtio_config_ops virtio_pci_config_ops = {
 	.find_vq	= vp_find_vq,
 	.del_vq		= vp_del_vq,
 	.get_features	= vp_get_features,
-	.set_features	= vp_set_features,
+	.finalize_features = vp_finalize_features,
 };
 
 /* the PCI probing function */
diff --git a/drivers/virtio/virtio_ring.c b/drivers/virtio/virtio_ring.c
index 72bf8bc0901..6eb5303fed1 100644
--- a/drivers/virtio/virtio_ring.c
+++ b/drivers/virtio/virtio_ring.c
@@ -18,6 +18,7 @@
  */
 #include <linux/virtio.h>
 #include <linux/virtio_ring.h>
+#include <linux/virtio_config.h>
 #include <linux/device.h>
 
 #ifdef DEBUG
@@ -87,8 +88,11 @@ static int vring_add_buf(struct virtqueue *_vq,
 	if (vq->num_free < out + in) {
 		pr_debug("Can't add buf len %i - avail = %i\n",
 			 out + in, vq->num_free);
-		/* We notify *even if* VRING_USED_F_NO_NOTIFY is set here. */
-		vq->notify(&vq->vq);
+		/* FIXME: for historical reasons, we force a notify here if
+		 * there are outgoing parts to the buffer.  Presumably the
+		 * host should service the ring ASAP. */
+		if (out)
+			vq->notify(&vq->vq);
 		END_USE(vq);
 		return -ENOSPC;
 	}
@@ -320,4 +324,19 @@ void vring_del_virtqueue(struct virtqueue *vq)
 }
 EXPORT_SYMBOL_GPL(vring_del_virtqueue);
 
+/* Manipulates transport-specific feature bits. */
+void vring_transport_features(struct virtio_device *vdev)
+{
+	unsigned int i;
+
+	for (i = VIRTIO_TRANSPORT_F_START; i < VIRTIO_TRANSPORT_F_END; i++) {
+		switch (i) {
+		default:
+			/* We don't understand this bit. */
+			clear_bit(i, vdev->features);
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(vring_transport_features);
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index ccb78f66c2b..48399e134c0 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -788,8 +788,6 @@ config WATCHDOG_RIO
 	  machines.  The watchdog timeout period is normally one minute but
 	  can be changed with a boot-time parameter.
 
-# V850 Architecture
-
 # XTENSA Architecture
 
 #
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 25b352b664d..edd305a64e6 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -119,8 +119,6 @@ obj-$(CONFIG_SH_WDT) += shwdt.o
 
 # SPARC64 Architecture
 
-# V850 Architecture
-
 # XTENSA Architecture
 
 # Architecture Independant
diff --git a/drivers/zorro/zorro-sysfs.c b/drivers/zorro/zorro-sysfs.c
index 3da712cc770..5290552d2ef 100644
--- a/drivers/zorro/zorro-sysfs.c
+++ b/drivers/zorro/zorro-sysfs.c
@@ -15,7 +15,6 @@
 #include <linux/zorro.h>
 #include <linux/stat.h>
 #include <linux/string.h>
-#include <linux/fs.h>
 
 #include "zorro.h"